├── AUTHORS ├── .clang-format ├── ChangeLog ├── NEWS ├── .gitlab-ci.yml ├── src ├── soth.cpp ├── Givens.cpp ├── Makefile ├── Allocator.hpp ├── api.hpp ├── Allocator.cpp ├── Random.cpp ├── BaseY.cpp ├── solvers.hpp ├── Algebra.cpp ├── Bound.hpp ├── Random.hpp ├── debug.cpp ├── CMakeLists.txt ├── BasicStage.cpp ├── BasicStage.hpp ├── HCOD.hpp ├── Algebra.hpp ├── BaseY.hpp ├── Bound.cpp ├── ActiveSet.cpp ├── Stage.hpp └── debug.hpp ├── doc ├── pictures │ ├── HRP2.jpg │ ├── soth.tif │ ├── footer.jpg │ └── footer.txt ├── header.html ├── footer.html ├── Doxyfile.extra.in ├── additionalDoc │ └── package.hh └── package.css ├── .gitmodules ├── autogen.sh ├── INSTALL ├── exe ├── CMakeLists.txt ├── simple.cpp ├── ijrr │ ├── ihqp.cpp │ └── ehqp.cpp └── conic_simplification.cpp ├── .travis.yml ├── README.md ├── unitTesting ├── tbasic.cpp ├── tasearch.cpp ├── gettimeofday.hpp ├── gettimeofday.cpp ├── tstage.cpp ├── thcod.cpp ├── tdestructiveQR.cpp ├── tcod.cpp ├── tdump_problem.cpp ├── RandomGenerator.hpp ├── CMakeLists.txt ├── tgivens.cpp ├── COD.hpp ├── tchrono.cpp ├── ttrack.cpp └── tsubmatrix.cpp ├── CMakeLists.txt ├── .travis └── build ├── CMakeModules └── FindEigen3.cmake └── COPYING /AUTHORS: -------------------------------------------------------------------------------- 1 | Authors: 2 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | ... 4 | -------------------------------------------------------------------------------- /ChangeLog: -------------------------------------------------------------------------------- 1 | Write in this file the modifications you commit. 2 | -------------------------------------------------------------------------------- /NEWS: -------------------------------------------------------------------------------- 1 | Write in this file the news related to package dg-tutorial. 2 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | include: http://rainboard.laas.fr/project/soth/.gitlab-ci.yml 2 | -------------------------------------------------------------------------------- /src/soth.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010 CNRS 3 | * 4 | * Nicolas Mansard 5 | */ 6 | -------------------------------------------------------------------------------- /doc/pictures/HRP2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/soth/HEAD/doc/pictures/HRP2.jpg -------------------------------------------------------------------------------- /doc/pictures/soth.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/soth/HEAD/doc/pictures/soth.tif -------------------------------------------------------------------------------- /doc/pictures/footer.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/soth/HEAD/doc/pictures/footer.jpg -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "cmake"] 2 | path = cmake 3 | url = git://github.com/jrl-umi3218/jrl-cmakemodules.git 4 | -------------------------------------------------------------------------------- /autogen.sh: -------------------------------------------------------------------------------- 1 | echo "" 2 | echo "This package is installed through cmake. Please read INSTALL for instructions." 3 | echo "" 4 | -------------------------------------------------------------------------------- /doc/pictures/footer.txt: -------------------------------------------------------------------------------- 1 | Copy in this directory the image you wish to use in the footer of the documentation or edit file sot-core/doc/footer.html and 2 | remove this file. 3 | -------------------------------------------------------------------------------- /doc/header.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | soth library documentation 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /doc/footer.html: -------------------------------------------------------------------------------- 1 |

2 |
3 |
4 | 5 |
soth library documentation
6 |
7 |
8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /doc/Doxyfile.extra.in: -------------------------------------------------------------------------------- 1 | INPUT = @CMAKE_SOURCE_DIR@/src \ 2 | @CMAKE_SOURCE_DIR@/doc/additionalDoc 3 | IMAGE_PATH = @CMAKE_SOURCE_DIR@/doc/pictures 4 | 5 | FILE_PATTERNS = *.cc *.cpp *.h *.hpp *.hxx 6 | 7 | TAGFILES = \ 8 | -------------------------------------------------------------------------------- /INSTALL: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 3 | # 4 | 5 | Installation instructions for library dg-tutorial 6 | ---------------------------------------------- 7 | 8 | It is recommended to create a specific directory to install this package. 9 | mkdir build 10 | cd build 11 | cmake [Options] .. 12 | make 13 | make install 14 | 15 | Options: 16 | 17 | -DCMAKE_INSTALL_PREFIX=... 18 | specifies the directory where to install the package. 19 | 20 | -DCMAKE_BUILD_TYPE=[none|debug|release|relwithdebinfo|MinSizeRel] 21 | specifies to type of compilation (release, debug, ...) 22 | -------------------------------------------------------------------------------- /src/Givens.cpp: -------------------------------------------------------------------------------- 1 | #include "soth/Givens.hpp" 2 | 3 | namespace soth { 4 | Givens::Givens() : i(0), j(0) {} 5 | 6 | Givens::Givens(double a, double b, Index i, Index j, double* z) : i(i), j(j) { 7 | makeGivens(a, b, i, j, z); 8 | } 9 | 10 | void Givens::makeGivens(double a, double b, Index i, Index j, double* z) { 11 | G.makeGivens(a, b, z); 12 | this->i = i; 13 | this->j = j; 14 | Gt = G.adjoint(); 15 | } 16 | 17 | GivensSequence& GivensSequence::push(const Givens& g) { 18 | G.push_back(g); 19 | return *this; 20 | } 21 | 22 | } // namespace soth 23 | -------------------------------------------------------------------------------- /src/Makefile: -------------------------------------------------------------------------------- 1 | all: lib header 2 | 3 | 4 | 5 | TEMPLATE = 6 | 7 | SRC = \ 8 | Soth.cpp 9 | 10 | HEADER = \ 11 | Soth.h Algebra.h 12 | 13 | LIB_DIR = ../lib 14 | LIB = ${LIB_DIR}/libsoth.so 15 | 16 | # --- CFLAGS 17 | CFLAGS = -I/usr/include -I${PWD}/../include 18 | 19 | 20 | # --- MAIN RULES 21 | 22 | header: ${HEADER:%=../include/soth/%} ${TEMPLATE:%=../include/soth/%} 23 | 24 | lib: ${LIB} 25 | 26 | # --- HEADERS 27 | 28 | ../include/soth/%.h: %.h 29 | @ln -snf ${PWD}/$< $@ 30 | 31 | ../include/soth/%.t.cpp: %.t.cpp 32 | @ln -snf ${PWD}/$< $@ 33 | 34 | # --- LIB 35 | 36 | ../.objs/%.o: %.cpp 37 | echo $@ $^ ${CFLAGS} 38 | g++ -o $@ -c $< ${CFLAGS} 39 | 40 | 41 | ${LIB}: ${SRC:%.cpp=../.objs/%.o} 42 | g++ -o $@ -shared $^ 43 | -------------------------------------------------------------------------------- /src/Allocator.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __SOTH_ALLOCATOR__ 2 | #define __SOTH_ALLOCATOR__ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace soth { 11 | class SOTH_EXPORT AllocatorML { 12 | typedef Eigen::MatrixXd::Index Index; 13 | typedef std::list resource_t; 14 | typedef resource_t::iterator resource_iter_t; 15 | 16 | resource_t resource; 17 | Index max; 18 | 19 | public: 20 | AllocatorML(Index max) : resource(), max(max) {} 21 | 22 | void reset(); 23 | void resetTop(Index min); 24 | Index get(); 25 | void put(const Index& token); 26 | void disp(std::ostream& os) const; 27 | 28 | SOTH_EXPORT friend std::ostream& operator<<(std::ostream& os, 29 | const AllocatorML& aml); 30 | }; 31 | 32 | } // namespace soth 33 | 34 | #endif // #ifndef __SOTH_ALLOCATOR__ 35 | -------------------------------------------------------------------------------- /exe/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS}) 2 | INCLUDE_DIRECTORIES(${CMAKE_BINARY_DIR}/include) 3 | LINK_DIRECTORIES(${Boost_LIBRARY_DIRS}) 4 | 5 | # Static library that will avoid useless recompilation. 6 | #add_library(utils STATIC 7 | # ../unitTesting/gettimeofday.cpp ../unitTesting/RandomGenerator.cpp 8 | #) 9 | 10 | # --- tsubmatrix --- 11 | SET(EXECUTABLE_NAME conic_simplification) 12 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 13 | conic_simplification.cpp) 14 | ADD_DEPENDENCIES(${EXECUTABLE_NAME} ${PROJECT_NAME}) 15 | ADD_DEPENDENCIES(${EXECUTABLE_NAME} utils) 16 | TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${PROJECT_NAME} utils) 17 | 18 | # --- tbasic --- 19 | SET(EXECUTABLE_NAME simple) 20 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 21 | simple.cpp) 22 | ADD_DEPENDENCIES(${EXECUTABLE_NAME} ${PROJECT_NAME}) 23 | ADD_DEPENDENCIES(${EXECUTABLE_NAME} utils) 24 | TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${PROJECT_NAME} utils) 25 | 26 | -------------------------------------------------------------------------------- /doc/additionalDoc/package.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | * This file is part of sot-dynamic. 9 | * sot-dynamic is free software: you can redistribute it and/or 10 | * modify it under the terms of the GNU Lesser General Public License 11 | * as published by the Free Software Foundation, either version 3 of 12 | * the License, or (at your option) any later version. 13 | * sot-dynamic is distributed in the hope that it will be 14 | * useful, but WITHOUT ANY WARRANTY; without even the implied warranty 15 | * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU Lesser General Public License for more details. You should 17 | * have received a copy of the GNU Lesser General Public License along 18 | * with sot-dynamic. If not, see . 19 | */ 20 | 21 | /** \mainpage 22 | \section soth_section_introduction Introduction 23 | 24 | The soth package is ... 25 | 26 | **/ 27 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | env: 2 | global: 3 | secure: JIUFWD7JojZ6b1mmtbf4/lj5pEYGO403nYuH6yHjQyZYIsKzvZhN2s6rI/HsrenCYVI5nLvZjgTN3uD8uJS06QerQo/RxwPiBKik/MbQeFMjBGuMeZLG7A1ewvo6FXD4n2WZoif6Q5N3jUQxv1uSijcX+tqtCoGlV/0davGcl+M= 4 | after_success: 5 | - coveralls -e _travis/install -e tests 6 | - git config --global user.name "Travis CI" 7 | - git config --global user.email "thomas.moulard+travis@gmail.com" 8 | - git remote set-url origin https://thomas-moulard:${GH_TOKEN}@github.com/stack-of-tasks/soth.git 9 | - git fetch origin gh-pages:gh-pages 10 | - cd _travis/build/doc && ../../../cmake/github/update-doxygen-doc.sh 11 | before_install: 12 | - git submodule update --init --recursive 13 | - sudo apt-get update -qq 14 | - sudo apt-get install -qq doxygen doxygen-latex libboost-all-dev libeigen3-dev 15 | - sudo pip install cpp-coveralls 16 | script: ./.travis/build 17 | language: cpp 18 | branches: 19 | only: 20 | - master 21 | compiler: 22 | - clang 23 | - gcc 24 | notifications: 25 | email: 26 | - hpp-source@laas.fr 27 | matrix: 28 | allow_failures: 29 | - compiler: clang 30 | -------------------------------------------------------------------------------- /src/api.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2010, 3 | * François Bleibel, 4 | * Olivier Stasse, 5 | * 6 | * CNRS/AIST 7 | * 8 | * This file is part of sot-core. 9 | * sot-core is free software: you can redistribute it and/or 10 | * modify it under the terms of the GNU Lesser General Public License 11 | * as published by the Free Software Foundation, either version 3 of 12 | * the License, or (at your option) any later version. 13 | * sot-core is distributed in the hope that it will be 14 | * useful, but WITHOUT ANY WARRANTY; without even the implied warranty 15 | * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU Lesser General Public License for more details. You should 17 | * have received a copy of the GNU Lesser General Public License along 18 | * with sot-core. If not, see . 19 | */ 20 | 21 | #ifndef SOTH_API_HH 22 | #define SOTH_API_HH 23 | 24 | #if defined(WIN32) 25 | #ifdef soth_EXPORTS 26 | #define SOTH_EXPORT __declspec(dllexport) 27 | #else 28 | #define SOTH_EXPORT __declspec(dllimport) 29 | #endif 30 | #else 31 | #define SOTH_EXPORT 32 | #endif 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | soth 2 | ==== 3 | 4 | [![Building Status](https://travis-ci.org/stack-of-tasks/soth.svg?branch=master)](https://travis-ci.org/stack-of-tasks/soth) 5 | [![Pipeline status](https://gepgitlab.laas.fr/stack-of-tasks/soth/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/stack-of-tasks/soth/commits/master) 6 | [![Coverage report](https://gepgitlab.laas.fr/stack-of-tasks/soth/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/stack-of-tasks/doc/stack-of-tasks/soth/master/coverage/) 7 | 8 | 9 | Setup 10 | ----- 11 | 12 | To compile this package, it is recommended to create a separate build 13 | directory: 14 | 15 | mkdir _build 16 | cd _build 17 | cmake [OPTIONS] .. 18 | make install 19 | 20 | Please note that CMake produces a `CMakeCache.txt` file which should 21 | be deleted to reconfigure a package from scratch. 22 | 23 | 24 | ### Dependencies 25 | 26 | The matrix abstract layer depends on several packages which 27 | have to be available on your machine. 28 | 29 | - Libraries: 30 | - eigen3 31 | - System tools: 32 | - CMake (>=2.6) 33 | - pkg-config 34 | - usual compilation tools (GCC/G++, make, etc.) 35 | -------------------------------------------------------------------------------- /unitTesting/tbasic.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Unittest of the BasicStage class. 4 | * 5 | * -------------------------------------------------------------------------- */ 6 | 7 | //#define SOTH_DEBUG 8 | //#define SOTH_DEBUG_MODE 45 9 | #include "soth/BaseY.hpp" 10 | #include "soth/BasicStage.hpp" 11 | #include "soth/debug.hpp" 12 | 13 | using namespace soth; 14 | using std::endl; 15 | 16 | /* -------------------------------------------------------------------------- */ 17 | 18 | void testBasicStage() { 19 | MatrixXd m1(5, 4); 20 | Map map1(m1.data(), m1.size(), 1); 21 | map1 = VectorXd::LinSpaced((long int)m1.size(), 0.0, (double)m1.size() - 1); 22 | std::cout << "m1 = " << m1 << endl; 23 | 24 | VectorBound b1(5); 25 | b1[0] = 1.6; 26 | b1[1] = std::make_pair(-0.1, 0.2); 27 | std::cout << "b1 = " << b1 << endl; 28 | 29 | soth::BaseY Y(5); 30 | soth::BasicStage st(5, 4, m1.data(), b1.data(), Y); 31 | // st.set( m1.data(),b1.data() ); 32 | st.set(m1, b1); 33 | 34 | std::cout << "J=" << st.getJ() << endl; 35 | std::cout << "b=" << st.getBounds() << endl; 36 | } 37 | 38 | int main(int, char**) { testBasicStage(); } 39 | -------------------------------------------------------------------------------- /src/Allocator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace soth { 5 | /* Replace all the tokens in the resource. */ 6 | void AllocatorML::reset() { resetTop(0); } 7 | 8 | /* Replace the tokens [min,max[ in the ressource. */ 9 | void AllocatorML::resetTop(Index min) { 10 | assert(min <= max); 11 | resource.resize(max - min); 12 | Index inc = min; 13 | for (resource_iter_t iter = resource.begin(); iter != resource.end(); 14 | ++iter) { 15 | (*iter) = inc++; 16 | } 17 | assert(resource.size() == 0 || resource.back() == max - 1); 18 | } 19 | 20 | typename AllocatorML::Index AllocatorML::get() { 21 | assert(resource.size() > 0); 22 | const Index token = resource.front(); 23 | resource.pop_front(); 24 | return token; 25 | } 26 | 27 | void AllocatorML::put(const Index& token) { 28 | resource.push_front(token); 29 | assert((Index)resource.size() <= max); 30 | } 31 | 32 | void AllocatorML::disp(std::ostream& os) const { 33 | typedef resource_t::const_iterator resource_citer_t; 34 | os << "[ "; 35 | for (resource_citer_t iter = resource.begin(); iter != resource.end(); 36 | ++iter) { 37 | os << *iter << " "; 38 | } 39 | os << " ];"; 40 | } 41 | 42 | std::ostream& operator<<(std::ostream& os, const AllocatorML& aml) { 43 | aml.disp(os); 44 | return os; 45 | } 46 | 47 | } // namespace soth 48 | -------------------------------------------------------------------------------- /src/Random.cpp: -------------------------------------------------------------------------------- 1 | #define SOTH_DEBUG 2 | #define SOTH_DEBUG_MODE 45 3 | #include "soth/debug.hpp" 4 | 5 | #include 6 | #include "Random.hpp" 7 | 8 | #ifdef WIN32 9 | inline double round(double d) { return floor(d + 0.5); } 10 | #endif /* WIN32 */ 11 | 12 | namespace soth { 13 | unsigned int Random::current = 33331; 14 | const unsigned int Random::SOTH_RND_MAX = 2147483647; // RAND_MAX; 15 | 16 | unsigned int Random::next() { 17 | // static const unsigned long long int m = static_cast(MULT); const unsigned long long int c = m * current; current = 19 | // (unsigned int)c % SOTH_RND_MAX; return current; 20 | return std::rand() % SOTH_RND_MAX; 21 | } 22 | 23 | void Random::setSeed(unsigned int newSeed) { srand(newSeed); } 24 | //{current = newSeed;} 25 | 26 | // Simulate a white noise with mean 0 and var 1. 27 | double whiteNoise(void) { 28 | const int ACC = 100; 29 | double x = 0; 30 | for (int i = 0; i < ACC; ++i) x = x + Random::rand(); 31 | return (x - ACC / 2.) * sqrt(12.0 / ACC); 32 | } 33 | // Simulate any white noise. 34 | int whiteNoise(int mean, double var) { 35 | double x = whiteNoise() * var + mean; 36 | return std::max(0, (int)round(x)); 37 | } 38 | // Simulate a discrete uniform law inside [ bmin,bmax ] (each bound 39 | // being reached). 40 | int randu(int bmin, int bmax) { 41 | assert(bmin < bmax); 42 | double X = Random::rand(); 43 | sotDEBUG(1) << "X=" << X << std::endl; 44 | return (int)floor((bmax - bmin + 1) * X + bmin); 45 | } 46 | 47 | } // namespace soth 48 | -------------------------------------------------------------------------------- /src/BaseY.cpp: -------------------------------------------------------------------------------- 1 | #include "soth/BaseY.hpp" 2 | 3 | namespace soth { 4 | // Empty construction with memory allocation. 5 | BaseY::BaseY(const Index& insize) 6 | : isExplicit(false), 7 | size(insize), 8 | rank(0), 9 | matrixExplicit(size, size), 10 | householderEssential(size, size) { 11 | householderEssential.setZero(); 12 | } 13 | 14 | void BaseY::computeExplicitly() { 15 | isExplicit = true; 16 | matrixExplicit = getHouseholderSequence(); 17 | // std::cout << matrixExplicit << std::endl; exit(0); 18 | } 19 | 20 | BaseY& BaseY::operator*=(const Givens& g) { 21 | assert(isExplicit && 22 | "you can't add Givens rotation to Y if the householder rotation " 23 | "matrix has not been explicitly computed"); 24 | g.applyThisOnTheLeft(matrixExplicit); 25 | return *this; 26 | } 27 | 28 | BaseY& BaseY::operator*=(const GivensSequence& G) { 29 | assert(isExplicit && 30 | "you can't add Givens rotation to Y if the householder rotation " 31 | "matrix has not been explicitly computed"); 32 | G.applyThisOnTheLeft(matrixExplicit); 33 | return *this; 34 | } 35 | 36 | //// Y *= Yup. Carefull: there is some recopy here. 37 | // void BaseY:: 38 | // composeOnTheRight( const BaseY& Yp ) 39 | //{ 40 | // /* TODO */ 41 | // throw "TODO"; 42 | //} 43 | //// Y *= HHup. 44 | 45 | // void BaseY:: 46 | // composeOnTheRight( const HouseholderSequence & hh ) 47 | // { 48 | // if( isExplicit ) 49 | // { 50 | ///* TODO */ 51 | // throw "TODO"; 52 | // } 53 | // else 54 | // { 55 | // matrixHH.append(hh); 56 | // } 57 | 58 | // } 59 | 60 | } // namespace soth 61 | -------------------------------------------------------------------------------- /unitTesting/tasearch.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Active search unittest. Could be asserted. 4 | * 5 | * -------------------------------------------------------------------------- */ 6 | 7 | #include "RandomGenerator.hpp" 8 | #include "soth/HCOD.hpp" 9 | #include "soth/debug.hpp" 10 | 11 | int main(int, char**) { 12 | soth::sotDebugTrace::openFile(); 13 | const unsigned int NB_STAGE = 3; 14 | const int RANKFREE[] = {5, 2, 5, 5, 3}; 15 | // const int RANKFREE[] = { 5, 4, 5, 5, 3 }; 16 | const int RANKLINKED[] = {0, 2, 1, 5, 3}; 17 | const int NR[] = {5, 4, 5, 5, 8}; 18 | const unsigned int NC = 15; 19 | 20 | /* Initialize J and b. */ 21 | std::vector J(NB_STAGE); 22 | std::vector b(NB_STAGE); 23 | soth::generateDeficientDataSet(J, b, NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 24 | 25 | b[0][2] = std::make_pair(-0.3, .3); 26 | b[0][3] = std::make_pair(-0.4, .4); 27 | b[0][4] = std::make_pair(-0.5, .5); 28 | 29 | for (unsigned int i = 0; i < NB_STAGE; ++i) { 30 | std::cout << "J" << i + 1 << " = " << (soth::MATLAB)J[i] << std::endl; 31 | std::cout << "e" << i + 1 << " = " << b[i] << ";" << std::endl; 32 | } 33 | // assert( std::abs(J[0](0,0)-(-1.1149))<1e-5 ); 34 | 35 | /* SOTH structure construction. */ 36 | soth::HCOD hcod(NC, NB_STAGE); 37 | for (unsigned int i = 0; i < NB_STAGE; ++i) { 38 | hcod.pushBackStage(J[i], b[i]); 39 | assert(NR[i] > 0); 40 | } 41 | hcod.setInitialActiveSet(); 42 | hcod.setNameByOrder("stage_"); 43 | 44 | Eigen::VectorXd solution; 45 | hcod.activeSearch(solution); 46 | hcod.show(std::cout); 47 | } 48 | -------------------------------------------------------------------------------- /unitTesting/gettimeofday.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2008, 2009, 2010, 3 | * 4 | * Francois Keith 5 | * Olivier Stasse 6 | * 7 | * JRL, CNRS/AIST 8 | * 9 | * This file is part of walkGenJrl. 10 | * walkGenJrl is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU Lesser General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * walkGenJrl is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Lesser Public License for more details. 19 | * You should have received a copy of the GNU Lesser General Public License 20 | * along with walkGenJrl. If not, see . 21 | * 22 | * Research carried out within the scope of the 23 | * Joint Japanese-French Robotics Laboratory (JRL) 24 | */ 25 | 26 | #ifndef PORTABILITY_GETTIMEOFDAY_HH 27 | #define PORTABILITY_GETTIMEOFDAY_HH 28 | 29 | // This deals with gettimeofday portability issues. 30 | #ifdef HAVE_SYS_TIME_H 31 | #include 32 | #else 33 | #ifdef WIN32 34 | #include 35 | #include 36 | 37 | struct timezone { 38 | int tz_minuteswest; /* minutes W of Greenwich */ 39 | int tz_dsttime; /* type of dst correction */ 40 | }; 41 | 42 | int gettimeofday(struct timeval *tv, struct timezone *tz); 43 | 44 | #else // WIN32 45 | #error "gettimeof day does not seem to be supported on your platform." 46 | #endif // WIN32 47 | #endif //! HAVE_SYS_TIME_H 48 | #endif //! JRL_WALKGEN_PORTABILITY_GETTIMEOFDAY_HH 49 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2010 CNRS 3 | # 4 | 5 | CMAKE_MINIMUM_REQUIRED(VERSION 2.6) 6 | 7 | INCLUDE(cmake/base.cmake) 8 | INCLUDE(cmake/boost.cmake) 9 | INCLUDE(cmake/cpack.cmake) 10 | INCLUDE(cmake/eigen.cmake) 11 | 12 | SET(PROJECT_NAME soth) 13 | SET(PROJECT_DESCRIPTION "...") 14 | SET(PROJECT_URL "http://github.com/laas/soth") 15 | 16 | # Disable -Werror on Unix for now. 17 | SET(CXX_DISABLE_WERROR True) 18 | SET(CMAKE_VERBOSE_MAKEFILE true) 19 | 20 | SET(DOXYGEN_USE_MATHJAX YES) 21 | 22 | # C++ version 4.7 is less permissive than older versions. 23 | # To avoid the created errors, the flag -fpermissive is added. 24 | # TODO: remove this patch 25 | IF(UNIX) 26 | exec_program(${CMAKE_CXX_COMPILER} 27 | ARGS ${CMAKE_CXX_COMPILER_ARG1} -dumpversion 28 | OUTPUT_VARIABLE _cxx_COMPILER_VERSION 29 | ) 30 | string(REGEX REPLACE "([0-9])\\.([0-9])(\\.[0-9])?" "\\1\\2" 31 | _cxx_COMPILER_VERSION ${_cxx_COMPILER_VERSION}) 32 | 33 | IF(${_cxx_COMPILER_VERSION} GREATER 46 ) 34 | SET(ADD_CXX_PATCH TRUE) 35 | ADD_DEFINITIONS("-fpermissive") 36 | ENDIF(${_cxx_COMPILER_VERSION} GREATER 46 ) 37 | ENDIF(UNIX) 38 | 39 | SETUP_PROJECT() 40 | SEARCH_FOR_EIGEN() 41 | 42 | # Handle OS specificities 43 | INCLUDE(CheckIncludeFiles) 44 | CHECK_INCLUDE_FILES("sys/time.h" SYS_TIME_H) 45 | IF(SYS_TIME_H) 46 | ADD_DEFINITIONS("-DHAVE_SYS_TIME_H") 47 | ENDIF(SYS_TIME_H) 48 | 49 | # --- BOOST HEADER --------------------------------------------------- 50 | # Search for dependencies. 51 | # Boost 52 | SET(BOOST_COMPONENTS program_options signals) 53 | SEARCH_FOR_BOOST() 54 | 55 | ADD_SUBDIRECTORY(src) 56 | ADD_SUBDIRECTORY(unitTesting) 57 | ADD_SUBDIRECTORY(exe) 58 | 59 | IF(ADD_CXX_PATCH) 60 | PKG_CONFIG_APPEND_CFLAGS("-fpermissive") 61 | ENDIF(ADD_CXX_PATCH) 62 | PKG_CONFIG_APPEND_CFLAGS(${_Eigen_CFLAGS}) 63 | PKG_CONFIG_APPEND_LIBS("soth") 64 | 65 | SETUP_PROJECT_FINALIZE() 66 | SETUP_PROJECT_CPACK() 67 | 68 | -------------------------------------------------------------------------------- /unitTesting/gettimeofday.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2008, 2009, 2010, 3 | * 4 | * Francois Keith 5 | * Olivier Stasse 6 | * 7 | * JRL, CNRS/AIST 8 | * 9 | * This file is part of walkGenJrl. 10 | * walkGenJrl is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU Lesser General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * walkGenJrl is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Lesser Public License for more details. 19 | * You should have received a copy of the GNU Lesser General Public License 20 | * along with walkGenJrl. If not, see . 21 | * 22 | * Research carried out within the scope of the 23 | * Joint Japanese-French Robotics Laboratory (JRL) 24 | */ 25 | #ifdef WIN32 26 | #include "gettimeofday.hpp" 27 | 28 | #include 29 | #if defined(_MSC_VER) || defined(_MSC_EXTENSIONS) 30 | #define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64 31 | #else 32 | #define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL 33 | #endif 34 | 35 | int gettimeofday(struct timeval *tv, struct timezone *tz) { 36 | FILETIME ft; 37 | unsigned __int64 tmpres = 0; 38 | static int tzflag; 39 | 40 | if (NULL != tv) { 41 | GetSystemTimeAsFileTime(&ft); 42 | 43 | tmpres |= ft.dwHighDateTime; 44 | tmpres <<= 32; 45 | tmpres |= ft.dwLowDateTime; 46 | 47 | /*converting file time to unix epoch*/ 48 | tmpres /= 10; /*convert into microseconds*/ 49 | tmpres -= DELTA_EPOCH_IN_MICROSECS; 50 | tv->tv_sec = (long)(tmpres / 1000000UL); 51 | tv->tv_usec = (long)(tmpres % 1000000UL); 52 | } 53 | 54 | if (NULL != tz) { 55 | if (!tzflag) { 56 | _tzset(); 57 | tzflag++; 58 | } 59 | tz->tz_minuteswest = _timezone / 60; 60 | tz->tz_dsttime = _daylight; 61 | } 62 | 63 | return 0; 64 | } 65 | 66 | #endif /*WIN32*/ 67 | -------------------------------------------------------------------------------- /src/solvers.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __SOTH_SOLVERS__ 2 | #define __SOTH_SOLVERS__ 3 | 4 | #include 5 | 6 | namespace soth { 7 | using namespace Eigen; 8 | 9 | /* Solve x in the problem Ax=b, for generic matrix and vector A and b, with 10 | * A= and b=, and store the result in v. A is considered to be 11 | * upper triangular and full rank. Its strictly upper part is considered to 12 | * be 0 and the solver performs a forward substitution. 13 | */ 14 | template 15 | inline void solveInPlaceWithLowerTriangular(const MatrixBase& lhs, 16 | MatrixBase& rhs) { 17 | /* Forward substitution, colum version. */ 18 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixBase) 19 | assert(lhs.rows() == lhs.cols()); 20 | assert(lhs.rows() > 0); 21 | assert(rhs.size() == lhs.rows()); 22 | const int n = (int)lhs.rows(); 23 | for (int i = 0; i < n - 1; ++i) { 24 | rhs[i] /= lhs(i, i); 25 | rhs.tail(n - i - 1) -= rhs[i] * lhs.col(i).tail(n - i - 1); 26 | } 27 | rhs[n - 1] /= lhs(n - 1, n - 1); 28 | } 29 | 30 | /* Solve x in the problem Ax=b, for generic matrix and vector A and b, with 31 | * A= and b=, and store the result in v. A is considered to be 32 | * upper triangular and full rank. Its strictly lower part is considered to 33 | * be 0 and the solver performs a backward substitution. 34 | */ 35 | template 36 | inline void solveInPlaceWithUpperTriangular(const MatrixBase& lhs, 37 | MatrixBase& rhs) { 38 | /* Backward substitution, colum version. */ 39 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatrixBase) 40 | assert(lhs.rows() == lhs.cols()); 41 | assert(lhs.rows() > 0); 42 | assert(rhs.size() == lhs.rows()); 43 | const int n = (int)lhs.rows(); 44 | for (int i = n - 1; i > 0; --i) { 45 | rhs[i] /= lhs(i, i); 46 | rhs.head(i) -= rhs[i] * lhs.col(i).head(i); 47 | } 48 | rhs[0] /= lhs(0, 0); 49 | } 50 | } // namespace soth 51 | 52 | #endif // #ifndef __SOTH_SOLVERS__ 53 | -------------------------------------------------------------------------------- /unitTesting/tstage.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Simple test of the Stage class. 4 | * No assertion, assert could be added on the final test. 5 | * 6 | * -------------------------------------------------------------------------- */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "RandomGenerator.hpp" 13 | #include "soth/BaseY.hpp" 14 | #include "soth/Stage.hpp" 15 | 16 | int main(int, char**) { 17 | // const int NB_STAGE = 5; 18 | // const int RANK[] = { 3, 5, 3, 5, 3 }; 19 | // const int NR[] = { 5, 5, 5, 5, 8 }; 20 | // const int NC = 20; 21 | 22 | const unsigned int NB_STAGE = 3; 23 | const int RANK[] = {3, 4, 3, 5, 3}; 24 | const int RANKLINKED[] = {0, 0, 0, 0, 0}; 25 | const int NR[] = {5, 4, 5, 5, 8}; 26 | const unsigned int NC = 12; 27 | /* Initialize J and b. */ 28 | std::vector J(NB_STAGE); 29 | std::vector b(NB_STAGE); 30 | 31 | soth::generateDeficientDataSet(J, b, NB_STAGE, RANK, RANKLINKED, NR, NC); 32 | 33 | for (unsigned int i = 0; i < NB_STAGE; ++i) { 34 | std::cout << "J" << i << " = " << (soth::MATLAB)J[i] << std::endl; 35 | } 36 | 37 | /* SOTH structure construction. */ 38 | soth::BaseY Y(NC); 39 | typedef boost::shared_ptr stage_ptr_t; 40 | typedef std::vector stage_list_t; 41 | stage_list_t stages(NB_STAGE); 42 | for (unsigned int i = 0; i < NB_STAGE; ++i) { 43 | std::cout << " --- STAGE " << i << " --------------------------------- " 44 | << std::endl; 45 | /* Compute the initial COD for each stage. */ 46 | stages[i] = stage_ptr_t(new soth::Stage(J[i], b[i], Y)); 47 | #ifndef NDEBUG 48 | stages[i]->reset(); 49 | #endif 50 | stages[i]->setInitialActiveSet(); 51 | stages[i]->computeInitialCOD(Y); 52 | Eigen::MatrixXd Jrec; 53 | stages[i]->recompose(Jrec); 54 | std::cout << "Jrec" << i << " = " << (soth::MATLAB)Jrec << std::endl; 55 | } 56 | 57 | Eigen::MatrixXd rec; 58 | stages[0]->recompose(rec); 59 | } 60 | -------------------------------------------------------------------------------- /.travis/build: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | set -ev 3 | 4 | # Directories. 5 | root_dir=`pwd` 6 | build_dir="$root_dir/_travis/build" 7 | install_dir="$root_dir/_travis/install" 8 | 9 | # Shortcuts. 10 | git_clone="git clone --quiet --recursive" 11 | 12 | # Create layout. 13 | rm -rf "$build_dir" "$install_dir" 14 | mkdir -p "$build_dir" 15 | mkdir -p "$install_dir" 16 | 17 | # Setup environment variables. 18 | export LD_LIBRARY_PATH="$install_dir/lib:$LD_LIBRARY_PATH" 19 | export LD_LIBRARY_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`:$LD_LIBRARY_PATH" 20 | export PKG_CONFIG_PATH="$install_dir/lib/pkgconfig:$PKG_CONFIG_PATH" 21 | export PKG_CONFIG_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`/pkgconfig:$PKG_CONFIG_PATH" 22 | 23 | install_dependency() 24 | { 25 | echo "--> Compiling $1" 26 | mkdir -p "$build_dir/$1" 27 | cd "$build_dir" 28 | $git_clone "git://github.com/$1" "$1" 29 | cd "$build_dir/$1" 30 | cmake . -DCMAKE_INSTALL_PREFIX:STRING="$install_dir" 31 | make install 32 | } 33 | 34 | # Install dependencies 35 | 36 | # Detect Ubuntu release 37 | if [ -f /etc/lsb-release ]; then 38 | (. /etc/lsb-release; 39 | echo "#!/bin/bash" > /tmp/local.bash 40 | echo "export DISTRIB_CODENAME="$DISTRIB_CODENAME >> /tmp/local.bash; 41 | echo "export DISTRIB_RELEASE="$DISTRIB_RELEASE >> /tmp/local.bash; 42 | echo "export DISTRIB_DESCRIPTION=\""$DISTRIB_DESCRIPTION\" >> /tmp/local.bash;) 43 | . /tmp/local.bash 44 | fi 45 | 46 | # Compile and run tests 47 | cd "$build_dir" 48 | 49 | if [ "$DISTRIB_RELEASE" = "14.04" ]; then 50 | echo "Build with 14.04" 51 | cmake "$root_dir" -DCMAKE_INSTALL_PREFIX="$install_dir" \ 52 | -DCMAKE_CXX_FLAGS="--coverage" \ 53 | -DCMAKE_EXE_LINKER_FLAGS="--coverage" \ 54 | -DCMAKE_MODULE_LINKER_FLAGS="--coverage" \ 55 | -DCMAKE_INSTALL_PREFIX:STRING="$install_dir" 56 | fi 57 | 58 | if [ "$DISTRIB_RELEASE" = "12.04" ]; then 59 | cmake "$root_dir" -DCMAKE_INSTALL_PREFIX="$install_dir" \ 60 | -DCMAKE_CXX_FLAGS="--coverage" \ 61 | -DCMAKE_EXE_LINKER_FLAGS="--coverage" \ 62 | -DCMAKE_INSTALL_PREFIX:STRING="$install_dir" 63 | fi 64 | 65 | make 66 | make test 67 | make install 68 | -------------------------------------------------------------------------------- /src/Algebra.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "soth/Algebra.hpp" 3 | 4 | using namespace soth; 5 | 6 | // MATLAB::MATLAB( const VectorXd& v1 ) 7 | // { 8 | // std::ostringstream os; os << "[ "; 9 | // for( unsigned int i=0;i 32 | void initMATLABFromBubMatrix(MATLAB& matlab, const bubTemplateMatrix& m1) { 33 | std::ostringstream os; 34 | os.precision(MATLAB::precision); 35 | os << "[ "; 36 | std::ostringstream ostmp; 37 | ostmp.precision(MATLAB::precision); 38 | for (unsigned int i = 0; i < m1.rows(); ++i) { 39 | for (unsigned int j = 0; j < m1.cols(); ++j) { 40 | if (m1(i, j) < 0) 41 | ostmp << "-"; 42 | else 43 | ostmp << " "; 44 | if (MATLAB::fullPrec || fabs(m1(i, j)) > 1e-6) 45 | ostmp << fabs(m1(i, j)); 46 | else { 47 | ostmp << "0"; 48 | } 49 | if (m1.cols() != j + 1) { 50 | ostmp << ","; 51 | const int size = ostmp.str().length(); 52 | for (unsigned int i = size; i < 8; ++i) ostmp << " "; 53 | ostmp << "\t"; 54 | } 55 | os << ostmp.str(); 56 | ostmp.str(""); 57 | } 58 | if (m1.rows() != i + 1) { 59 | os << " ;" << std::endl << " "; 60 | } else { 61 | os << " ];"; 62 | } 63 | } 64 | matlab.str = os.str(); 65 | } 66 | 67 | // MATLAB::MATLAB( const MatrixXd& m1) 68 | // {initMATLABFromBubMatrix(*this,m1);} 69 | 70 | namespace soth { 71 | bool MATLAB::fullPrec = false; 72 | unsigned int MATLAB::precision = 6; 73 | 74 | MATLAB::MATLAB(const double& x) { 75 | std::ostringstream os; 76 | os << x << " ; "; 77 | str = os.str(); 78 | } 79 | 80 | std::ostream& operator<<(std::ostream& os, const MATLAB& m) { 81 | return os << m.str; 82 | } 83 | } // namespace soth 84 | -------------------------------------------------------------------------------- /unitTesting/thcod.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Simple test of the HCOD class with assertion. 4 | * 5 | * -------------------------------------------------------------------------- */ 6 | #define SOTH_DEBUG 7 | #define SOTH_DEBUG_MODE 10 8 | #define SOTH_TEMPLATE_DEBUG_MODE 10 9 | 10 | #include "RandomGenerator.hpp" 11 | #include "soth/HCOD.hpp" 12 | #include "soth/Random.hpp" 13 | #include "soth/debug.hpp" 14 | 15 | using namespace soth; 16 | 17 | int main(int, char**) { 18 | soth::sotDebugTrace::openFile(); 19 | soth::Random::setSeed(7); 20 | const int NB_STAGE = 3; 21 | const int RANKFREE[] = {3, 4, 3, 5, 3}; 22 | const int RANKLINKED[] = {2, 2, 1, 5, 3}; 23 | const int NR[] = {5, 4, 5, 5, 8}; 24 | const int NC = 12; 25 | 26 | /* Initialize J and b. */ 27 | std::vector J(NB_STAGE); 28 | std::vector b(NB_STAGE); 29 | soth::generateDeficientDataSet(J, b, NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 30 | b[0][1] = std::make_pair(-0.1, 2.37); 31 | for (int i = 0; i < NB_STAGE; ++i) { 32 | sotDEBUG(0) << "J" << i + 1 << " = " << (soth::MATLAB)J[i] << std::endl; 33 | sotDEBUG(0) << "e" << i + 1 << " = " << b[i] << ";" << std::endl; 34 | } 35 | std::cout << J[0](0, 0) << std::endl; 36 | assert(std::abs(J[0](0, 0) - 0.544092) < 1e-5); 37 | 38 | /* SOTH structure construction. */ 39 | soth::HCOD hcod(NC, NB_STAGE); 40 | for (int i = 0; i < NB_STAGE; ++i) { 41 | hcod.pushBackStage(J[i], b[i]); 42 | assert(NR[i] > 0); 43 | } 44 | hcod.setInitialActiveSet(); 45 | hcod.setNameByOrder("stage_"); 46 | 47 | hcod.initialize(); 48 | hcod.Y.computeExplicitly(); 49 | hcod.computeSolution(true); 50 | std::cout << hcod.rank() << " " << hcod[0].rank() << " " << hcod[1].rank() 51 | << " " << hcod[2].rank() << std::endl; 52 | assert((hcod.rank() == 4) && (hcod[0].rank() == 0) && (hcod[1].rank() == 2) && 53 | (hcod[2].rank() == 2)); 54 | 55 | double tau = hcod.computeStepAndUpdate(); 56 | hcod.makeStep(tau); 57 | std::cout << "tau:" << tau << " " << std::abs(tau - 0.486522) << " " 58 | << soth::Stage::EPSILON << std::endl; 59 | assert((std::abs(tau - 0.486522) <= 50 * soth::Stage::EPSILON) && 60 | "Check bound test failed."); 61 | 62 | hcod.computeLagrangeMultipliers(hcod.nbStages()); 63 | bool testL = hcod.testLagrangeMultipliers(hcod.nbStages(), std::cout); 64 | sotDEBUG(5) << "Test multipliers: " << ((testL) ? "Passed!" : "Failed...") 65 | << std::endl; 66 | assert(testL && "Lagrange Multipliers test failed."); 67 | } 68 | -------------------------------------------------------------------------------- /src/Bound.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __SOTH_BOUND__ 2 | #define __SOTH_BOUND__ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace soth { 11 | class SOTH_EXPORT Bound { 12 | public: 13 | enum bound_t { 14 | BOUND_NONE, 15 | BOUND_INF, 16 | BOUND_SUP, 17 | BOUND_DOUBLE, 18 | BOUND_TWIN // equality 19 | }; 20 | 21 | protected: 22 | bound_t type; 23 | // In case of twin bounds, the value is stored in valInf. 24 | double valInf, valSup; 25 | double& valTwin; 26 | 27 | public: 28 | Bound(void); 29 | Bound(const Bound& clone); 30 | Bound(const double& val, bound_t type); 31 | Bound(const double& inValInf, const double& inValSup); 32 | Bound(const double& valTwin); 33 | 34 | const bound_t& getType(void) const { return type; } 35 | const double& getBound(bound_t type) const; 36 | /* Return the bound that is violated, NONE if bound are OK. 37 | * In case of twin-bounds, no check is performed, NONE is always returned. */ 38 | bound_t check(const double& val, const double& EPSILON = 0) const; 39 | /* Return the bound that is violated, NONE if bound are OK. 40 | * In case of twin-bounds, no check is performed, NONE is always returned. */ 41 | bound_t check(const double& val, std::pair damp, 42 | const double& EPSILON = 0) const; 43 | /* Return the bound b s.t. |b-val|& val); 52 | SOTH_EXPORT friend std::ostream& operator<<(std::ostream& os, const Bound&); 53 | 54 | }; // Class Bound 55 | 56 | typedef Eigen::Matrix VectorBound; 57 | // typedef std::pair ConstraintRef; 58 | struct ConstraintRef { 59 | Eigen::MatrixXd::Index row; 60 | Bound::bound_t type; 61 | ConstraintRef(Eigen::MatrixXd::Index r, Bound::bound_t t) : row(r), type(t) {} 62 | ConstraintRef(void) : row(-1), type(Bound::BOUND_NONE) {} 63 | double sign() const { return (type == Bound::BOUND_SUP) ? +1 : -1; } 64 | }; 65 | extern const ConstraintRef CONSTRAINT_VOID; 66 | typedef std::vector cstref_vector_t; 67 | typedef std::vector::size_type cstref_vector_size_t; 68 | 69 | SOTH_EXPORT std::ostream& operator<<(std::ostream& os, const VectorBound& t); 70 | SOTH_EXPORT std::ostream& operator<<(std::ostream& os, 71 | const ConstraintRef& cst); 72 | 73 | } // namespace soth 74 | 75 | #endif // #ifndef __SOTH_BOUND__ 76 | -------------------------------------------------------------------------------- /src/Random.hpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Random matrix generator. 4 | * 5 | * -------------------------------------------------------------------------- */ 6 | 7 | #include "soth/Algebra.hpp" 8 | 9 | namespace soth { 10 | class Random { 11 | static const unsigned int SOTH_RND_MAX; //= 4278255361U; 12 | static const unsigned int MULT = 1884103651; 13 | static unsigned int current; 14 | 15 | static unsigned int next(); 16 | 17 | public: 18 | static void setSeed(unsigned int newSeed); 19 | 20 | template 21 | static ReturnType rand(); 22 | 23 | template 24 | static ReturnType randMax(); 25 | }; 26 | 27 | template <> 28 | inline unsigned int Random::rand() { 29 | return next(); 30 | } 31 | template <> 32 | inline unsigned int Random::randMax() { 33 | return SOTH_RND_MAX; 34 | } 35 | template <> 36 | inline int Random::rand() { 37 | return next() >> 1; 38 | } 39 | template <> 40 | inline int Random::randMax() { 41 | return SOTH_RND_MAX >> 1; 42 | } 43 | template <> 44 | inline double Random::rand() { 45 | return static_cast(next()) / (SOTH_RND_MAX); 46 | } 47 | template <> 48 | inline double Random::randMax() { 49 | return 1.; 50 | } 51 | 52 | template 53 | struct rnd_traits { 54 | static Scalar max() { return Random::randMax(); } 55 | static Scalar default_min() { return Scalar(-1); } 56 | static Scalar default_max() { return Scalar(1); } 57 | }; 58 | 59 | template <> 60 | struct rnd_traits { 61 | static int max() { return Random::randMax(); } 62 | static int default_min() { return -10; } 63 | static int default_max() { return 11; } 64 | }; 65 | 66 | class MatrixRnd { 67 | public: 68 | template 69 | static MatrixBase& randomize( 70 | MatrixBase& m, 71 | typename Derived::Scalar min = 72 | rnd_traits::default_min(), 73 | typename Derived::Scalar max = 74 | rnd_traits::default_max()) { 75 | for (typename Derived::Index i = 0; i < m.rows(); ++i) { 76 | for (typename Derived::Index j = 0; j < m.cols(); ++j) 77 | m(i, j) = static_cast( 78 | (max - min) * Random::rand()) + 79 | min; 80 | } 81 | return m; 82 | } 83 | }; 84 | 85 | // Simulate a white noise with mean 0 and var 1. 86 | double whiteNoise(void); 87 | // Simulate any white noise. 88 | int whiteNoise(int mean, double var); 89 | // Simulate a discrete uniform law inside [ bmin,bmax ] (each bound 90 | // being reached). 91 | int randu(int bmin, int bmax); 92 | 93 | } // namespace soth. 94 | -------------------------------------------------------------------------------- /src/debug.cpp: -------------------------------------------------------------------------------- 1 | /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 2 | * Copyright Projet Lagadic, 2005 3 | *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 4 | * 5 | * File: sotDebug.h 6 | * Project: STack of Tasks 7 | * Author: Nicolas Mansard 8 | * 9 | * Version control 10 | * =============== 11 | * 12 | * $Id$ 13 | * 14 | * Description 15 | * ============ 16 | * 17 | * Macro de trace et de debugage 18 | * 19 | * - TRACAGE: TRACE et ERROR_TRACE fonctionnent comme des printf 20 | * avec retour chariot en fin de fonction. 21 | * CERROR et CTRACE fonctionnent comme les flux de sortie 22 | * C++ cout et cerr. 23 | * - DEBUGAGE: DEBUG_TRACE(niv, et DERROR_TRACE(niv, fonctionnent 24 | * comme des printf, n'imprimant que si le niveau de trace 'niv' est 25 | * superieur au mode de debugage VP_DEBUG_MODE. 26 | * CDEBUG(niv) fonctionne comme le flux de sortie C++ cout. 27 | * DEBUG_ENABLE(niv) vaut 1 ssi le niveau de tracage 'niv' 28 | * est superieur au mode de debugage DEBUG_MODE. Il vaut 0 sinon. 29 | * - PROG DEFENSIVE: DEFENSIF(a) vaut a ssi le mode defensif est active, 30 | * et vaut 0 sinon. 31 | * 32 | * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 33 | 34 | #include "soth/debug.hpp" 35 | #include 36 | #include 37 | #include 38 | 39 | namespace soth { 40 | #ifdef WIN32 41 | const char* sotDebugTrace::DEBUG_FILENAME_DEFAULT = "traces.txt"; 42 | #else /*WIN32*/ 43 | const char* sotDebugTrace::DEBUG_FILENAME_DEFAULT = "/tmp/traces.txt"; 44 | #endif /*WIN32*/ 45 | 46 | #ifdef SOTH_DEBUG 47 | std::ofstream debugfile(sotDebugTrace::DEBUG_FILENAME_DEFAULT, 48 | std::ios::trunc& std::ios::out); 49 | #else 50 | std::ofstream debugfile; //( "/dev/null", std::ios::trunc&std::ios::out ); 51 | class __sotDebug_init { 52 | public: 53 | __sotDebug_init(void) { 54 | debugfile.setstate(std::ios::failbit); /* debugfile.close(); */ 55 | } 56 | }; 57 | __sotDebug_init __sotDebug_initialisator; 58 | 59 | #endif 60 | 61 | sotDebugTrace sotDEBUGFLOW(debugfile); 62 | sotDebugTrace sotERRORFLOW(debugfile); 63 | 64 | void sotDebugTrace::openFile(const char* filename) { 65 | if (debugfile.good() && debugfile.is_open()) debugfile.close(); 66 | debugfile.clear(); 67 | debugfile.open(filename, std::ios::trunc & std::ios::out); 68 | // std::cout << filename << debugfile.good() << debugfile.is_open() << 69 | // std::endl; 70 | } 71 | 72 | void sotDebugTrace::closeFile(const char* /*filename*/) { 73 | if (debugfile.good() && debugfile.is_open()) { 74 | debugfile.close(); 75 | } 76 | debugfile.setstate(std::ios::failbit); 77 | } 78 | 79 | // sotDebugTrace sotDEBUGFLOW(std::cout); 80 | // sotDebugTrace sotERRORFLOW(std::cerr); 81 | 82 | } // namespace soth 83 | -------------------------------------------------------------------------------- /unitTesting/tdestructiveQR.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Exhaustive test of the DestructiveColPivQR class. Assertion should be added. 4 | * 5 | * -------------------------------------------------------------------------- */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "soth/DestructiveColPivQR.hpp" 12 | 13 | using namespace Eigen; 14 | 15 | void testColPivForCod() { 16 | const int n = 7; 17 | const int m = 6; 18 | MatrixXd M; 19 | M = MatrixXd::Random(m, m - 2) * MatrixXd::Random(m - 2, n); 20 | MatrixXd Y = MatrixXd::Zero(n, n); 21 | 22 | std::cout << "initial matrix M: " << std::endl; 23 | std::cout << M << std::endl << std::endl << std::endl; 24 | 25 | std::cout << "***** Reference Decomposition of M' *****" << std::endl; 26 | ColPivHouseholderQR ref(M.transpose()); 27 | MatrixXd resR = ref.matrixQR().triangularView(); 28 | MatrixXd resQ = ref.matrixQR().triangularView(); 29 | std::cout << "R_ref = " << std::endl; 30 | std::cout << resR << std::endl << std::endl; 31 | std::cout << "householder essential H_ref= " << std::endl; 32 | std::cout << resQ << std::endl << std::endl; 33 | 34 | std::cout << "***** Tested Decomposition of M' *****" << std::endl; 35 | Transpose Mt = M.transpose(); 36 | Block Yse = Y.block(0, 0, n, m); 37 | DestructiveColPivQR, Block > qr(Mt, Yse); 38 | std::cout << "R (in place, no transposition) = " << std::endl; 39 | std::cout << M.transpose() << std::endl << std::endl; 40 | std::cout << "permuted R = " << std::endl; 41 | std::cout << M.transpose() * qr.colsPermutation() << std::endl << std::endl; 42 | std::cout << "householder essential H= " << std::endl; 43 | std::cout << Y << std::endl << std::endl; 44 | std::cout << "QR = " << std::endl; 45 | std::cout << qr.householderQ() * M.transpose() << std::endl << std::endl; 46 | 47 | std::cout << std::endl << "***** Check *****" << std::endl; 48 | std::cout 49 | << "correct R (R_ref*P_ref' = R): " 50 | << ((resR * ref.colsPermutation().transpose() - M.transpose()).isZero() 51 | ? "true" 52 | : "false") 53 | << std::endl; 54 | std::cout << "correct householder (Href = H): " 55 | << ((resQ - Y.block(0, 0, n, m)).isZero() ? "true" : "false") 56 | << std::endl; 57 | std::cout << "correct permutations (P_ref*P = I): " 58 | << ((ref.colsPermutation().transpose() * qr.colsPermutation()) 59 | .toDenseMatrix() 60 | .isIdentity() 61 | ? "true" 62 | : "false") 63 | << std::endl; 64 | } 65 | 66 | int main() { 67 | testColPivForCod(); 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 2010 CNRS 3 | # 4 | # Author: Florent Lamiraux 5 | # 6 | 7 | SET(LIBRARY_NAME ${PROJECT_NAME}) 8 | IF(WIN32) 9 | ADD_DEFINITIONS("-DNOMINMAX") 10 | ENDIF(WIN32) 11 | 12 | INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS}) 13 | LINK_DIRECTORIES(${Boost_LIBRARY_DIRS}) 14 | # Boost_SIGNALS_LIBRARY 15 | 16 | SET(${PROJECT_NAME}_SRC 17 | # ActiveSet.cpp 18 | Allocator.cpp 19 | Algebra.cpp 20 | Bound.cpp 21 | BaseY.cpp 22 | HCOD.cpp 23 | Givens.cpp 24 | debug.cpp 25 | Stage.cpp 26 | BasicStage.cpp 27 | Random.cpp 28 | ) 29 | 30 | SET(${PROJECT_NAME}_HEADERS 31 | api.hpp 32 | ActiveSet.hpp 33 | Algebra.hpp 34 | Allocator.hpp 35 | BaseY.hpp 36 | BasicStage.hpp 37 | Bound.hpp 38 | debug.hpp 39 | DestructiveColPivQR.hpp 40 | Givens.hpp 41 | HCOD.hpp 42 | solvers.hpp 43 | Stage.hpp 44 | SubMatrix.hpp 45 | Random.hpp 46 | ) 47 | 48 | # ---------------------------------------------------- 49 | # --- INCLUDE ---------------------------------------- 50 | # ---------------------------------------------------- 51 | 52 | MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/soth") 53 | 54 | FOREACH(header ${${PROJECT_NAME}_HEADERS}) 55 | GET_FILENAME_COMPONENT(headerName ${header} NAME) 56 | IF(WIN32) 57 | execute_process(COMMAND ${CMAKE_COMMAND} -E copy_if_different 58 | ${${PROJECT_NAME}_SOURCE_DIR}/src/${header} 59 | ${${PROJECT_NAME}_BINARY_DIR}/include/${PROJECT_NAME}/ 60 | ) 61 | ELSE(WIN32) 62 | execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink 63 | ${${PROJECT_NAME}_SOURCE_DIR}/src/${header} 64 | ${${PROJECT_NAME}_BINARY_DIR}/include/${PROJECT_NAME}/${headerName}) 65 | ENDIF(WIN32) 66 | INSTALL(FILES ${${PROJECT_NAME}_SOURCE_DIR}/src/${header} 67 | DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME} 68 | PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE 69 | ) 70 | 71 | ENDFOREACH(header) 72 | 73 | # Install procedure for the header files 74 | #---------------------------------------------------- 75 | 76 | # ---------------------------------------------------- 77 | # --- LIBS ------------------------------------------- 78 | # ---------------------------------------------------- 79 | 80 | #INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIR}/include) 81 | INCLUDE_DIRECTORIES(${CMAKE_BINARY_DIR}/include) 82 | 83 | ADD_LIBRARY(${LIBRARY_NAME} 84 | SHARED 85 | ${${PROJECT_NAME}_SRC}) 86 | 87 | IF(NOT WIN32) 88 | TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${Boost_LIBRARIES}) 89 | ENDIF(NOT WIN32) 90 | 91 | SET_TARGET_PROPERTIES(${LIBRARY_NAME} 92 | PROPERTIES 93 | SOVERSION "${PROJECT_VERSION}" 94 | INSTALL_RPATH "${CMAKE_INSTALL_LIBDIR}") 95 | 96 | INSTALL(TARGETS ${LIBRARY_NAME} 97 | DESTINATION ${CMAKE_INSTALL_LIBDIR}) 98 | 99 | 100 | -------------------------------------------------------------------------------- /src/BasicStage.cpp: -------------------------------------------------------------------------------- 1 | #define SOTH_DEBUG 2 | #define SOTH_DEBUG_MODE 15 3 | #include "soth/BasicStage.hpp" 4 | #include "soth/debug.hpp" 5 | 6 | namespace soth { 7 | std::ostream& operator<<(std::ostream& os, const VectorBound& t) { 8 | os << "[ "; 9 | typedef VectorBound::Index Index; 10 | EI_FOREACH(i, t) { 11 | os << t[i]; 12 | if (i + 1 < t.size()) 13 | os << "; "; 14 | else 15 | os << " ]"; 16 | } 17 | return os; 18 | } 19 | 20 | /* --- STAGE -------------------------------------------------------------- */ 21 | 22 | BasicStage::BasicStage(const Index innr, const Index innc, const double* Jdata, 23 | const Bound* bdata, const BaseY& Y) 24 | : boundsInternal(), 25 | Jmap(Jdata, innr, innc), 26 | boundsMap(bdata, innr) 27 | 28 | , 29 | J(Jmap), 30 | bounds(boundsMap), 31 | nr(innr), 32 | nc(innc) 33 | 34 | , 35 | Y(Y) {} 36 | 37 | BasicStage::BasicStage(const Index innr, const Index innc, const double* Jdata, 38 | const BaseY& Y) 39 | : boundsInternal(innr), 40 | Jmap(Jdata, innr, innc), 41 | boundsMap(boundsInternal.data(), innr) 42 | 43 | , 44 | J(Jmap), 45 | bounds(boundsMap), 46 | nr(innr), 47 | nc(innc) 48 | 49 | , 50 | Y(Y) {} 51 | 52 | BasicStage::BasicStage(const MatrixXd& inJ, const VectorBound& inbounds, 53 | const BaseY& inY) 54 | : boundsInternal(), 55 | Jmap(inJ.data(), inJ.rows(), inJ.cols()), 56 | boundsMap(inbounds.data(), inbounds.size(), 1) 57 | 58 | , 59 | J(Jmap), 60 | bounds(boundsMap), 61 | nr((Index)inJ.rows()), 62 | nc((Index)inJ.cols()) 63 | 64 | , 65 | Y(inY) { 66 | assert(inbounds.size() == int(nr)); 67 | } 68 | 69 | void BasicStage::set(const MatrixXd& inJ, const VectorBound& inbounds) { 70 | assert(inJ.rows() == (int)nr && inJ.cols() == (int)nc); 71 | assert(inbounds.size() == (int)nr); 72 | sotDEBUG(15) << "inJ = " << (MATLAB)inJ << std::endl; 73 | set(inJ.data(), inbounds.data()); 74 | } 75 | 76 | void BasicStage::set(const double* Jdata, const Bound* bdata) { 77 | new (&Jmap) MapXd(Jdata, nr, nc); 78 | new (&boundsMap) MapBound(bdata, nr); 79 | 80 | sotDEBUG(15) << "map = " << (MATLAB)Jmap << std::endl; 81 | sotDEBUG(15) << "ref = " << (MATLAB)J << std::endl; 82 | sotDEBUG(15) << (&Jmap) << "=?=" << (&J) << std::endl; 83 | } 84 | 85 | MatrixXd BasicStage::getJ(void) const { return J; } 86 | 87 | VectorBound BasicStage::getBounds(void) const { return bounds; } 88 | 89 | VectorBound& BasicStage::getBoundsInternal() { 90 | assert(boundsInternal.data() == boundsMap.data()); 91 | return boundsInternal; 92 | } 93 | 94 | VectorXd BasicStage::getJrow(const Index& cst) const { return J.row(cst); } 95 | 96 | Bound BasicStage::getBoundRow(const Index& cst) const { return bounds[cst]; } 97 | 98 | } // namespace soth 99 | -------------------------------------------------------------------------------- /src/BasicStage.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __SOTH_BASIC_STAGE__ 2 | #define __SOTH_BASIC_STAGE__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include "soth/Algebra.hpp" 8 | #include "soth/Bound.hpp" 9 | #include "soth/api.hpp" 10 | 11 | #include 12 | #include 13 | 14 | #ifndef WITHOUT_NOTIFIOR 15 | 16 | #if BOOST_VERSION > 105300 17 | #include 18 | #define NOTIFIOR_SIGNAL2 19 | #else 20 | #include 21 | #endif 22 | 23 | #endif 24 | 25 | namespace soth { 26 | class BaseY; 27 | 28 | /* --- STAGE -------------------------------------------------------------- */ 29 | class SOTH_EXPORT BasicStage : boost::noncopyable { 30 | private: 31 | VectorBound boundsInternal; 32 | 33 | typedef Map MapXd; 34 | typedef Map MapBound; 35 | typedef MatrixXd::Index Index; 36 | 37 | MapXd Jmap; 38 | MapBound boundsMap; 39 | 40 | protected: 41 | typedef MapXd MatrixXdRef; 42 | typedef MapBound VectorBoundRef; 43 | 44 | const MatrixXdRef& J; 45 | const VectorBoundRef& bounds; 46 | const Index nr, nc; // nr=nbCols(J), nc=nbRows(J). 47 | const BaseY& Y; 48 | 49 | public: 50 | /* Constructor from references on the constraint matrix and 51 | * vector - no copy. */ 52 | BasicStage(const MatrixXd& J, const VectorBound& bounds, const BaseY& Y); 53 | /* Constructor from size and data maps. */ 54 | BasicStage(const Index nr, const Index nc, const double* Jdata, 55 | const Bound* bdata, const BaseY& Y); 56 | /* Same as upper, with bdata:=&boundsInternal. */ 57 | BasicStage(const Index nr, const Index nc, const double* Jdata, 58 | const BaseY& Y); 59 | 60 | /* Reset the data map from references - no copy. */ 61 | void set(const MatrixXd& J, const VectorBound& bounds); 62 | /* Reset the data map from pointers. */ 63 | void set(const double* Jdata, const Bound* bdata); 64 | 65 | Index nbConstraints(void) const { return nr; } 66 | 67 | /* Return the J row of the constraint (in the global ppol, 68 | * not only in the active pool. */ 69 | VectorXd getJrow(const Index& cst) const; 70 | /* Return the bound-values of constraint .*/ 71 | Bound getBoundRow(const Index& cst) const; 72 | 73 | public: /* For debug purpose, could be remove on RELEASE. */ 74 | std::string name; 75 | MatrixXd getJ() const; 76 | VectorBound getBounds() const; 77 | VectorBound& getBoundsInternal(); 78 | 79 | public: /* Notification, could be removed conditionnaly to the lack of 80 | boost::signal. */ 81 | #ifndef WITHOUT_NOTIFIOR 82 | typedef boost::function 83 | listener_function_t; 84 | #ifdef NOTIFIOR_SIGNAL2 85 | boost::signals2::signal 86 | notifior; 87 | #else 88 | boost::signal notifior; 89 | #endif 90 | #else 91 | inline void notifior(int, int, std::string) {} 92 | #endif 93 | }; 94 | 95 | } // namespace soth 96 | 97 | #endif // #ifndef __SOTH_BASIC_STAGE__ 98 | -------------------------------------------------------------------------------- /unitTesting/tcod.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Unittest of the COD class, with time comparison with the Jacobi class. Could 4 | * be asserted. 5 | * 6 | * -------------------------------------------------------------------------- */ 7 | #define SOTH_DEBUG 8 | #define SOTH_DEBUG_MODE 45 9 | #include "soth/debug.hpp" 10 | 11 | #include 12 | #include 13 | #include "COD.hpp" 14 | #include "gettimeofday.hpp" 15 | 16 | using namespace std; 17 | using namespace Eigen; 18 | using namespace soth; 19 | 20 | int main(void) { 21 | #ifndef NDEBUG 22 | sotDebugTrace::openFile(); 23 | #endif 24 | { 25 | // int NR=90,NC=60,RANK=30; 26 | // int NR=100,NC=100,RANK=100; 27 | int NR = 20, NC = 40, RANK = 6; 28 | 29 | MatrixXd Xhi = MatrixXd::Random(NR, RANK); 30 | MatrixXd R = MatrixXd::Random(RANK, NC); 31 | MatrixXd A; 32 | A = Xhi * R; 33 | 34 | COD Acod; 35 | Acod.compute(A); 36 | 37 | #ifndef NDEBUG 38 | cout << "A =" << (MATLAB)A << endl; 39 | std::cout << "rank = " << Acod.rank << std::endl; 40 | if (Acod.matrixU().cols() != 0) 41 | std::cout << "U = " << (MATLAB)Acod.matrixUr() << std::endl; 42 | else 43 | std::cout << "U = " << (MATLAB)Acod.matrixU() << std::endl; 44 | std::cout << "L = " << (MATLAB)(MatrixXd)Acod.matrixL() << std::endl; 45 | if (Acod.matrixV().cols() != 0) 46 | std::cout << "V = " << (MATLAB)Acod.matrixVr() << std::endl; 47 | else 48 | std::cout << "V = " << (MATLAB)Acod.matrixV() << std::endl; 49 | 50 | if ((Acod.matrixU().cols() != 0) && (Acod.matrixV().cols() != 0)) { 51 | std::cout << "ULV = " 52 | << (MATLAB)(MatrixXd)(Acod.matrixUr() * Acod.matrixL() * 53 | Acod.matrixVr().transpose()) 54 | << std::endl; 55 | std::cout << "ERR = " 56 | << (A - Acod.matrixUr() * Acod.matrixL() * 57 | Acod.matrixVr().transpose()) 58 | .norm() 59 | << std::endl; 60 | } 61 | #endif 62 | 63 | /* --- CHRONO --- */ 64 | struct timeval t0, t1; 65 | double totalTime = 0; 66 | double totalTimeSVD = 0; 67 | 68 | for (int shoot = 0; shoot < 1000; ++shoot) { 69 | Xhi = MatrixXd::Random(NR, RANK); 70 | R = MatrixXd::Random(RANK, NC); 71 | A = Xhi * R; 72 | 73 | gettimeofday(&t0, NULL); 74 | COD Acod; 75 | Acod.compute(A, RANK); 76 | gettimeofday(&t1, NULL); 77 | double time = (double)(t1.tv_sec - t0.tv_sec) + 78 | (double)(t1.tv_usec - t0.tv_usec) / 1.0e6; 79 | totalTime += time; 80 | 81 | gettimeofday(&t0, NULL); 82 | // JacobiSVD Asvd(A, ComputeThinU | ComputeThinV); 83 | gettimeofday(&t1, NULL); 84 | 85 | double timeSVD = (double)(t1.tv_sec - t0.tv_sec) + 86 | (double)(t1.tv_usec - t0.tv_usec) / 1.0e6; 87 | totalTimeSVD += timeSVD; 88 | if (!(shoot % 100)) 89 | std::cout << time * 1000 << "\t" << timeSVD * 1000 << std::endl; 90 | } 91 | std::cout << totalTime << "\t" << totalTimeSVD << std::endl; 92 | } 93 | return 0; 94 | } 95 | -------------------------------------------------------------------------------- /CMakeModules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | 33 | # --- MACRO CHECK --- 34 | macro(_eigen3_check_version) 35 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 36 | 37 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 41 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 42 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 43 | 44 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 45 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK FALSE) 47 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | set(EIGEN3_VERSION_OK TRUE) 49 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 50 | 51 | if(NOT EIGEN3_VERSION_OK) 52 | 53 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 54 | "but at least version ${Eigen3_FIND_VERSION} is required") 55 | endif(NOT EIGEN3_VERSION_OK) 56 | endmacro(_eigen3_check_version) 57 | 58 | 59 | # --- SEARCH AND CHECK --- 60 | if (EIGEN3_INCLUDE_DIR) 61 | # in cache already 62 | _eigen3_check_version() 63 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 64 | else (EIGEN3_INCLUDE_DIR) 65 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 66 | PATHS 67 | ${CMAKE_INSTALL_PREFIX}/include 68 | ${KDE4_INCLUDE_DIR} 69 | PATH_SUFFIXES eigen3 eigen 70 | ) 71 | if(EIGEN3_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN3_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 77 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 78 | endif(EIGEN3_INCLUDE_DIR) 79 | 80 | -------------------------------------------------------------------------------- /unitTesting/tdump_problem.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Generate a .txt file with a random problem. The key of the randomizer is 4 | * given by command line. 5 | * 6 | * -------------------------------------------------------------------------- */ 7 | #define SOTH_DEBUG 8 | #define SOTH_DEBUG_MODE 45 9 | #include "RandomGenerator.hpp" 10 | #include "soth/HCOD.hpp" 11 | #include "soth/Random.hpp" 12 | #include "soth/debug.hpp" 13 | 14 | #ifndef WIN32 15 | #include 16 | #endif // WIN32 17 | 18 | #include 19 | #include 20 | #include "gettimeofday.hpp" 21 | 22 | using namespace soth; 23 | using std::cerr; 24 | using std::cout; 25 | using std::endl; 26 | 27 | /* Compute [m1;m2] from m1 and m2 of same col number. */ 28 | template 29 | MatrixXd stack(const MatrixBase& m1, const MatrixBase& m2) { 30 | assert(m1.cols() == m2.cols()); 31 | const int m1r = m1.rows(), m2r = m2.rows(), mr = m1r + m2r, mc = m1.cols(); 32 | MatrixXd res(mr, mc); 33 | for (int i = 0; i < m1r; ++i) 34 | for (int j = 0; j < mc; ++j) res(i, j) = m1(i, j); 35 | for (int i = 0; i < m2r; ++i) 36 | for (int j = 0; j < mc; ++j) res(m1r + i, j) = m2(i, j); 37 | return res; 38 | } 39 | 40 | /* -------------------------------------------------------------------------- */ 41 | /* -------------------------------------------------------------------------- */ 42 | /* -------------------------------------------------------------------------- */ 43 | int main(int argc, char** argv) { 44 | { 45 | struct timeval tv; 46 | gettimeofday(&tv, NULL); 47 | 48 | int seed = (int)(tv.tv_usec % 7919); //= 7594; 49 | if (argc == 2) { 50 | seed = atoi(argv[1]); 51 | } 52 | std::cout << "seed = " << seed << std::endl; 53 | soth::Random::setSeed(seed); 54 | } 55 | 56 | /* Decide the size of the problem. */ 57 | unsigned int NB_STAGE, NC; 58 | std::vector NR, RANKLINKED, RANKFREE; 59 | soth::generateRandomProfile(NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 60 | 61 | /* Initialize J and b. */ 62 | std::vector J(NB_STAGE); 63 | std::vector b(NB_STAGE); 64 | generateDeficientDataSet(J, b, NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 65 | 66 | std::ofstream fout("/tmp/soth.txt"); 67 | fout << "variable size " << NC << endl << endl; 68 | 69 | for (unsigned int s = 0; s < NB_STAGE; ++s) { 70 | fout << "level" << endl << endl; 71 | 72 | int nbEqualities = 0; 73 | for (unsigned int r = 0; r < NR[s]; ++r) { 74 | if (b[s][r].getType() == Bound::BOUND_TWIN) nbEqualities++; 75 | } 76 | 77 | fout << "equalities " << nbEqualities << endl << endl; 78 | for (unsigned int r = 0; r < NR[s]; ++r) { 79 | if (b[s][r].getType() == Bound::BOUND_TWIN) { 80 | fout << J[s].row(r) << " " << b[s][r].getBound(Bound::BOUND_TWIN) 81 | << endl; 82 | } 83 | } 84 | fout << endl << "inequalities " << NR[s] - nbEqualities << endl << endl; 85 | for (unsigned int r = 0; r < NR[s]; ++r) { 86 | switch (b[s][r].getType()) { 87 | case Bound::BOUND_TWIN: 88 | break; 89 | case Bound::BOUND_INF: 90 | fout << J[s].row(r) << " " << b[s][r].getBound(Bound::BOUND_INF) 91 | << " 1e25" << endl; 92 | break; 93 | case Bound::BOUND_SUP: 94 | fout << J[s].row(r) << " " << -1e25 << " " 95 | << b[s][r].getBound(Bound::BOUND_SUP) << endl; 96 | break; 97 | case Bound::BOUND_DOUBLE: 98 | fout << J[s].row(r) << " " << b[s][r].getBound(Bound::BOUND_INF) 99 | << " " << b[s][r].getBound(Bound::BOUND_SUP) << endl; 100 | break; 101 | case Bound::BOUND_NONE: 102 | assert(false && "Never happening."); 103 | break; 104 | } 105 | } 106 | fout << endl; 107 | } 108 | 109 | fout << endl << "end" << endl << endl; 110 | } 111 | -------------------------------------------------------------------------------- /src/HCOD.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __SOTH_HCOD__ 2 | #define __SOTH_HCOD__ 3 | 4 | #include 5 | #include "soth/BaseY.hpp" 6 | #include "soth/Stage.hpp" 7 | #include "soth/api.hpp" 8 | 9 | namespace soth { 10 | class SOTH_EXPORT HCOD { 11 | protected: 12 | typedef boost::shared_ptr stage_ptr_t; 13 | typedef std::vector stage_sequence_t; 14 | typedef stage_sequence_t::iterator stage_iter_t; 15 | typedef stage_sequence_t::const_iterator stage_citer_t; 16 | typedef stage_sequence_t::reverse_iterator stage_riter_t; 17 | typedef stage_sequence_t::size_type stage_sequence_size_t; 18 | typedef MatrixXd::Index Index; 19 | 20 | public: 21 | HCOD(Index sizeProblem, Index nbStage = 0); 22 | 23 | void pushBackStage(const MatrixXd& J, const VectorBound& bounds); 24 | void pushBackStage(const Index& nr, const double* Jdata, const Bound* bdata); 25 | void pushBackStage(const Index& nr, const double* Jdata); 26 | void pushBackStages(const std::vector& J, 27 | const std::vector& bounds); 28 | 29 | Stage& stage(Index i); 30 | const Stage& stage(Index i) const; 31 | inline Stage& operator[](Index i) { return stage(i); } 32 | inline const Stage& operator[](Index i) const { return stage(i); } 33 | 34 | void setInitialActiveSet(); 35 | void setInitialActiveSet(const cstref_vector_t& Ir0, Index k); 36 | cstref_vector_t getOptimalActiveSet(Index k); 37 | std::vector getOptimalActiveSet(); 38 | void setInitialActiveSet(const std::vector& Ir); 39 | 40 | void useDamp(bool c) { withDamp = c; } 41 | bool useDamp() const { return withDamp; } 42 | void setDamping(const double& d); 43 | double getMaxDamping() const; 44 | 45 | // sizes 46 | Index sizeA() const; 47 | int rank() const; 48 | Index nbStages() const { return (Index)stages.size(); } 49 | 50 | /* --- Decomposition --- */ 51 | public: 52 | void reset(void); 53 | void initialize(void); 54 | void update(const Index& stageUp, const ConstraintRef& cst); 55 | void update(stage_iter_t stageIter, const ConstraintRef& cst); 56 | void downdate(const Index& stageDown, const Index& row); 57 | void downdate(stage_iter_t stageIter, const Index& row); 58 | 59 | protected: 60 | void updateY(const GivensSequence& Yup); 61 | 62 | /* --- Computations --- */ 63 | public: 64 | void damp(void); 65 | void computeSolution(bool compute_u = true); 66 | void computeLagrangeMultipliers(const Index& stageRef); 67 | double computeStepAndUpdate(void); 68 | double computeStep(void); 69 | bool searchAndDowndate(const Index& stageRef); 70 | bool search(const Index& stageRef); 71 | 72 | void makeStep(double tau, bool compute_u = true); 73 | void makeStep(bool compute_u = true); 74 | 75 | /* --- The big one --- */ 76 | public: 77 | // template< typename VectorGen > 78 | void activeSearch(VectorXd& u); 79 | 80 | /* --- Tests --- */ 81 | public: 82 | void show(std::ostream& os, bool check = false); 83 | void showActiveSet(std::ostream& os) const; 84 | bool testRecomposition(std::ostream* os = NULL); 85 | bool testSolution(std::ostream* os = NULL); 86 | bool testLagrangeMultipliers(Index stageRef, std::ostream* os = NULL) const; 87 | bool testLagrangeMultipliers(Index stageRef, std::ostream& os) const { 88 | return testLagrangeMultipliers(stageRef, &os); 89 | } 90 | 91 | void setNameByOrder(const std::string root = ""); 92 | void notifiorRegistration(const Stage::listener_function_t& f, 93 | int stageRank = -1); 94 | 95 | bool isDebugOnce; 96 | void debugOnce(std::string filename = "", bool keepOpen = false); 97 | 98 | protected: 99 | HCOD(void) : Y(0){}; 100 | 101 | protected: 102 | public: // DEBUG 103 | Index sizeProblem; 104 | soth::BaseY Y; 105 | stage_sequence_t stages; 106 | VectorXd solution; 107 | 108 | VectorXd uNext, Ytu, YtuNext, rho; 109 | int freezedStages; 110 | bool isReset, isInit, isSolutionCpt, withDamp; 111 | }; 112 | 113 | } // namespace soth 114 | 115 | #endif // #ifndef __SOTH_HCOD__ 116 | -------------------------------------------------------------------------------- /unitTesting/RandomGenerator.hpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Generation of random problems. 4 | * 5 | * -------------------------------------------------------------------------- */ 6 | 7 | #ifndef __SOTH_RANDOM_GENERATOR__ 8 | #define __SOTH_RANDOM_GENERATOR__ 9 | 10 | #include 11 | #include "soth/Algebra.hpp" 12 | #include "soth/Bound.hpp" 13 | 14 | namespace soth { 15 | /* Being specified the size of the problem NC, the number of stage NB_STAGE, 16 | * the size of each size NR, the rank of the stage wrt. the previous stages 17 | * RANKFREE = rank(Ji.Pi-1), and the rank due to the previous stage 18 | * RANKLINKED = rank(Ji) - RANKFREE, the function generates in J and b 19 | * a random problems with good conditionning numbers (ie =0 XOR >>0). 20 | */ 21 | void generateDeficientDataSet(std::vector &J, 22 | std::vector &b, 23 | const unsigned int NB_STAGE, 24 | const std::vector &RANKFREE, 25 | const std::vector &RANKLINKED, 26 | const std::vector &NR, 27 | const unsigned int NC); 28 | 29 | void generateDeficientDataSet(std::vector &J, 30 | std::vector &b, 31 | const unsigned int NB_STAGE, const int RANKFREE[], 32 | const int RANKLINKED[], const int NR[], 33 | const unsigned int NC); 34 | 35 | /* Generated randomly a profile of problem, ie sizes and ranks. The ouput 36 | * of this functions are to be sent to the previous function. */ 37 | void generateRandomProfile(unsigned int &nbStage, 38 | std::vector &rankfree, 39 | std::vector &ranklinked, 40 | std::vector &nr, unsigned int &nc); 41 | 42 | /* Input: size = number of cols, rowPercent = row number / col number, 43 | * rankPercent = rank / col number, 44 | * selfDeficiencyPercent = rankdef [A1 .. Ap] / sum rankdef(Ai) 45 | */ 46 | void generateFixedSizeRandomProfile( 47 | const unsigned int size, const double rowPercent, const double rankPercent, 48 | const double selfDeficiencyPercent, unsigned int &nbStage, 49 | std::vector &rankfree, std::vector &ranklinked, 50 | std::vector &nr, unsigned int &nc); 51 | 52 | void randomProblem(std::vector &J, 53 | std::vector &b); 54 | 55 | /* --- IN/OUT PROBLEMS --- */ 56 | void readProblemFromFile(const std::string name, 57 | std::vector &J, 58 | std::vector &b, 59 | unsigned int &NB_STAGE, std::vector &NR, 60 | unsigned int &NC); 61 | void readProblemFromFile(const std::string name, 62 | std::vector &J, 63 | std::vector &b); 64 | void readProblemFromBinFile(const std::string name, 65 | std::vector &J, 66 | std::vector &b, 67 | unsigned int &NB_STAGE, 68 | std::vector &NR, unsigned int &NC); 69 | 70 | void writeProblemToFile(const std::string name, 71 | const std::vector &J, 72 | const std::vector &b, 73 | const unsigned int &NB_STAGE, 74 | const std::vector &NR, 75 | const unsigned int &NC); 76 | 77 | void writeProblemToFile(const std::string name, 78 | const std::vector &J, 79 | const std::vector &b); 80 | } // namespace soth 81 | 82 | #endif // #ifndef __SOTH_RANDOM_GENERATOR__ 83 | -------------------------------------------------------------------------------- /exe/simple.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * IJRR 2013 tests: compare the cost of the HQP with the QP cascade of Kanoun 4 | * and DeLassa. 5 | * 6 | * -------------------------------------------------------------------------- */ 7 | #define SOTH_DEBUG 8 | #define SOTH_DEBUG_MODE 45 9 | #include "../unitTesting/COD.hpp" 10 | #include "../unitTesting/RandomGenerator.hpp" 11 | #include "soth/DestructiveColPivQR.hpp" 12 | #include "soth/HCOD.hpp" 13 | #include "soth/Random.hpp" 14 | #include "soth/debug.hpp" 15 | 16 | #include // for 'operator+=()' 17 | using namespace boost::assign; // bring 'operator+=()' into scope 18 | 19 | #ifndef WIN32 20 | #include 21 | #endif // WIN32 22 | 23 | #include 24 | #include 25 | #include "../unitTesting/gettimeofday.hpp" 26 | 27 | using namespace soth; 28 | using std::cerr; 29 | using std::cout; 30 | using std::endl; 31 | 32 | /* -------------------------------------------------------------------------- */ 33 | /* -------------------------------------------------------------------------- */ 34 | /* -------------------------------------------------------------------------- */ 35 | 36 | void ehqp(HCOD& hsolver) { 37 | hsolver.initialize(); 38 | // hsolver.Y.computeExplicitly(); 39 | // hsolver.computeSolution(); 40 | // hsolver.showActiveSet(std::cout); 41 | } 42 | 43 | /* -------------------------------------------------------------------------- */ 44 | /* -------------------------------------------------------------------------- */ 45 | /* -------------------------------------------------------------------------- */ 46 | 47 | int main() { 48 | unsigned int NB_STAGE, NC; 49 | std::vector NR, RANKLINKED, RANKFREE; 50 | std::vector J; 51 | std::vector b; 52 | 53 | soth::Random::setSeed(704819); 54 | // const int size = 100; 55 | 56 | // generateFixedSizeRandomProfile(size, 57 | // 1,0.99,0.99,NB_STAGE,RANKFREE,RANKLINKED,NR,NC); 58 | if (1) { 59 | NB_STAGE = 3; 60 | RANKFREE += 30, 40, 30; 61 | RANKLINKED += 0, 0, 0; 62 | NR = RANKFREE; 63 | NC = 100; 64 | } 65 | if (0) { 66 | NB_STAGE = 1; 67 | NC = 100; 68 | RANKFREE += NC; 69 | RANKLINKED += 0, 0, 0; 70 | NR = RANKFREE; 71 | } 72 | if (0) { 73 | RANKFREE += 6, 3, 7, 2, 5, 5, 44, 6, 2, 4, 5, 3, 7, 1, 50; 74 | RANKLINKED.resize(RANKFREE.size(), 0); 75 | NB_STAGE = (unsigned int)RANKFREE.size(); 76 | NR = RANKFREE; 77 | NC = 150; 78 | } 79 | 80 | std::cout << "nVar \t= " << NC << std::endl; 81 | std::cout << "nLevels \t= " << NB_STAGE << std::endl; 82 | std::cout << "LevelDim \t= [ "; 83 | for (int i = 0; i < (int)NB_STAGE; ++i) std::cout << NR[i] << " "; 84 | std::cout << "]" << std::endl; 85 | 86 | generateDeficientDataSet(J, b, NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 87 | 88 | HCOD hsolver(NC, NB_STAGE); 89 | VectorXd solution(NC); 90 | for (unsigned int i = 0; i < NB_STAGE; ++i) { 91 | for (int j = 0; j < b[i].size(); ++j) b[i][j] = Bound(rand()); 92 | hsolver.pushBackStage(J[i], b[i]); 93 | hsolver.setNameByOrder("level"); 94 | } 95 | hsolver.setDamping(0.0); 96 | hsolver.setInitialActiveSet(); 97 | 98 | struct timeval t0, t1; 99 | double time; 100 | 101 | gettimeofday(&t0, NULL); 102 | for (int i = 0; i < 1000; ++i) ehqp(hsolver); 103 | gettimeofday(&t1, NULL); 104 | time = (double)(t1.tv_sec - t0.tv_sec) + 105 | (double)(t1.tv_usec - t0.tv_usec) / 1.0e6; 106 | cout << "ehqp = " << time << " ms " << std::endl; 107 | 108 | MatrixXd A = J[0]; 109 | MatrixXd Y(NC, NC); 110 | 111 | gettimeofday(&t0, NULL); 112 | for (int i = 0; i < 1000; ++i) 113 | Eigen::DestructiveColPivQR mQR(A, Y, 1e-6); 114 | // Eigen::DestructiveColPivQR mQR(Ar,Y, 1e-6); 115 | gettimeofday(&t1, NULL); 116 | time = (double)(t1.tv_sec - t0.tv_sec) + 117 | (double)(t1.tv_usec - t0.tv_usec) / 1.0e6; 118 | cout << "qr = " << time << " ms " << std::endl; 119 | } 120 | -------------------------------------------------------------------------------- /unitTesting/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright 3 | # 4 | 5 | 6 | ADD_DEFINITIONS(-DDEBUG=2) 7 | IF(WIN32) 8 | ADD_DEFINITIONS("-DNOMINMAX") 9 | ENDIF(WIN32) 10 | 11 | INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS}) 12 | INCLUDE_DIRECTORIES(${CMAKE_BINARY_DIR}/include) 13 | LINK_DIRECTORIES(${Boost_LIBRARY_DIRS}) 14 | 15 | # Static library that will avoid useless recompilation. 16 | add_library(utils STATIC 17 | gettimeofday.cpp RandomGenerator.cpp 18 | ) 19 | 20 | # --- tsubmatrix --- 21 | SET(EXECUTABLE_NAME tsubmatrix) 22 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 23 | tsubmatrix.cpp) 24 | LIST (APPEND executable_list ${EXECUTABLE_NAME}) 25 | ADD_TEST(${EXECUTABLE_NAME} ${EXECUTABLE_NAME}) 26 | 27 | # --- tbasic --- 28 | SET(EXECUTABLE_NAME tbasic) 29 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 30 | tbasic.cpp) 31 | LIST (APPEND executable_list ${EXECUTABLE_NAME}) 32 | ADD_TEST(${EXECUTABLE_NAME} ${EXECUTABLE_NAME}) 33 | 34 | # --- tstage --- 35 | SET(EXECUTABLE_NAME tstage) 36 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 37 | tstage.cpp) 38 | ADD_DEPENDENCIES(${EXECUTABLE_NAME} utils) 39 | TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} utils) 40 | LIST (APPEND executable_list ${EXECUTABLE_NAME}) 41 | ADD_TEST(${EXECUTABLE_NAME} ${EXECUTABLE_NAME}) 42 | 43 | # --- tdestructiveQR --- 44 | SET(EXECUTABLE_NAME tdestructiveQR) 45 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 46 | tdestructiveQR.cpp) 47 | LIST (APPEND executable_list ${EXECUTABLE_NAME}) 48 | ADD_TEST(${EXECUTABLE_NAME} ${EXECUTABLE_NAME}) 49 | 50 | # --- thcod --- 51 | SET(EXECUTABLE_NAME thcod) 52 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 53 | thcod.cpp) 54 | ADD_DEPENDENCIES(${EXECUTABLE_NAME} utils) 55 | TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} utils) 56 | LIST (APPEND executable_list ${EXECUTABLE_NAME}) 57 | ADD_TEST(${EXECUTABLE_NAME} ${EXECUTABLE_NAME}) 58 | 59 | # --- thcod_full --- 60 | SET(EXECUTABLE_NAME texhcod) 61 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 62 | thcod_full.cpp) 63 | ADD_DEPENDENCIES(${EXECUTABLE_NAME} utils) 64 | TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} utils) 65 | LIST (APPEND executable_list ${EXECUTABLE_NAME}) 66 | # ADD_TEST(${EXECUTABLE_NAME} ${EXECUTABLE_NAME}) 67 | 68 | # --- tgivens --- 69 | SET(EXECUTABLE_NAME tgivens) 70 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 71 | tgivens.cpp) 72 | LIST (APPEND executable_list ${EXECUTABLE_NAME}) 73 | ADD_TEST(${EXECUTABLE_NAME} ${EXECUTABLE_NAME}) 74 | 75 | # --- tasearch --- 76 | SET(EXECUTABLE_NAME tasearch) 77 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 78 | tasearch.cpp) 79 | ADD_DEPENDENCIES(${EXECUTABLE_NAME} utils) 80 | TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} utils) 81 | LIST (APPEND executable_list ${EXECUTABLE_NAME}) 82 | ADD_TEST(${EXECUTABLE_NAME} ${EXECUTABLE_NAME}) 83 | 84 | # --- trandom --- 85 | SET(EXECUTABLE_NAME trandom) 86 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 87 | trandom.cpp) 88 | ADD_DEPENDENCIES(${EXECUTABLE_NAME} utils) 89 | TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} utils) 90 | LIST (APPEND executable_list ${EXECUTABLE_NAME}) 91 | IF(NOT WIN32) 92 | TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${Boost_PROGRAM_OPTIONS_LIBRARY}) 93 | ENDIF(NOT WIN32) 94 | ADD_TEST(${EXECUTABLE_NAME} ${EXECUTABLE_NAME}) 95 | 96 | # --- tcod --- 97 | SET(EXECUTABLE_NAME tcod) 98 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 99 | tcod.cpp) 100 | LIST (APPEND executable_list ${EXECUTABLE_NAME}) 101 | IF(NOT WIN32) 102 | TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${Boost_PROGRAM_OPTIONS_LIBRARY}) 103 | ENDIF(NOT WIN32) 104 | ADD_TEST(${EXECUTABLE_NAME} ${EXECUTABLE_NAME}) 105 | 106 | # --- tdamping --- 107 | # SET(EXECUTABLE_NAME tdamping) 108 | # ADD_EXECUTABLE(${EXECUTABLE_NAME} 109 | # tdamping.cpp) 110 | # ADD_DEPENDENCIES(${EXECUTABLE_NAME} utils) 111 | # TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} utils) 112 | # LIST (APPEND executable_list ${EXECUTABLE_NAME}) 113 | # IF(NOT WIN32) 114 | # TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} ${Boost_PROGRAM_OPTIONS_LIBRARY}) 115 | # ENDIF(NOT WIN32) 116 | 117 | # --- tchrono --- 118 | SET(EXECUTABLE_NAME tchrono) 119 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 120 | tchrono.cpp) 121 | ADD_DEPENDENCIES(${EXECUTABLE_NAME} utils) 122 | TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} utils) 123 | LIST (APPEND executable_list ${EXECUTABLE_NAME}) 124 | 125 | # --- tdump_problem --- 126 | SET(EXECUTABLE_NAME tdump_problem) 127 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 128 | tdump_problem.cpp) 129 | ADD_DEPENDENCIES(${EXECUTABLE_NAME} utils) 130 | TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} utils) 131 | LIST (APPEND executable_list ${EXECUTABLE_NAME}) 132 | 133 | # --- ttrack --- 134 | SET(EXECUTABLE_NAME ttrack) 135 | ADD_EXECUTABLE(${EXECUTABLE_NAME} 136 | ttrack.cpp) 137 | ADD_DEPENDENCIES(${EXECUTABLE_NAME} utils) 138 | TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} utils) 139 | LIST (APPEND executable_list ${EXECUTABLE_NAME}) 140 | #ADD_TEST(${EXECUTABLE_NAME} ${EXECUTABLE_NAME}) 141 | 142 | foreach (exec ${executable_list}) 143 | ADD_DEPENDENCIES(${exec} ${PROJECT_NAME}) 144 | TARGET_LINK_LIBRARIES(${exec} ${PROJECT_NAME}) 145 | endforeach(exec) 146 | -------------------------------------------------------------------------------- /doc/package.css: -------------------------------------------------------------------------------- 1 | body { 2 | font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif; 3 | color: #5D5D5D; 4 | } 5 | 6 | dl { 7 | border: 1.5px #82b6d7 solid; 8 | width: 97%; 9 | padding: 5px; 10 | color: #330077; 11 | } 12 | 13 | code { 14 | color: #3C9A35; 15 | } 16 | 17 | td.md { 18 | color: #0066CC; 19 | } 20 | 21 | h1 { 22 | padding-top: 50px; 23 | padding: 0px; 24 | font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif; 25 | font-variant: small-caps; 26 | color:#0066CC; 27 | text-align: center; 28 | } 29 | 30 | h2,h3,hr { 31 | magin-top: 15px; 32 | padding: 0px; 33 | font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif; 34 | font-variant: small-caps; 35 | color:#0066CC; 36 | } 37 | 38 | h4 { 39 | color: #3C9A35; 40 | } 41 | 42 | a:link { 43 | font-weight: bold; 44 | text-decoration: none; 45 | color:#0066CC 46 | } 47 | 48 | a:hover, a:active { 49 | text-decoration: underline; 50 | color: #3C9A35; 51 | } 52 | 53 | a:visited { 54 | font-weight: bold; 55 | color: #3C9A35; 56 | text-decoration: none; 57 | } 58 | 59 | DIV.memitem 60 | { 61 | spacing: 10px; 62 | width:100%; 63 | background:#FFFFFF; 64 | font-size:100%; 65 | line-height:normal; 66 | border-width: 1px; 67 | border-style: solid; 68 | border-color: #808080; 69 | -moz-border-radius: 8px 8px 8px 8px; 70 | } 71 | 72 | DIV.memproto 73 | { 74 | width:100%; 75 | background:#F0F0F0; 76 | font-size:100%; 77 | line-height:normal; 78 | border-width: 1px; 79 | border-style: solid; 80 | border-color: #808080; 81 | -moz-border-radius: 8px 8px 8px 8px; 82 | } 83 | 84 | DIV.memdoc 85 | { 86 | padding: 10px; 87 | width:100%; 88 | font-size:100%; 89 | line-height:normal; 90 | } 91 | 92 | DIV.tabs 93 | { 94 | float : left; 95 | width : 100%; 96 | background : url("tab_b.gif") repeat-x bottom; 97 | margin-bottom : 4px; 98 | } 99 | 100 | DIV.tabs UL 101 | { 102 | margin : 0px; 103 | padding-left : 10px; 104 | list-style : none; 105 | } 106 | 107 | DIV.tabs LI, DIV.tabs FORM 108 | { 109 | display : inline; 110 | margin : 0px; 111 | padding : 0px; 112 | } 113 | 114 | DIV.tabs FORM 115 | { 116 | float : right; 117 | } 118 | 119 | DIV.tabs A 120 | { 121 | float : left; 122 | background : url("tab_r.gif") no-repeat right top; 123 | border-bottom : 1px solid #84B0C7; 124 | font-size : x-small; 125 | font-weight : bold; 126 | text-decoration : none; 127 | } 128 | 129 | DIV.tabs A:hover 130 | { 131 | background-position: 100% -150px; 132 | } 133 | 134 | DIV.tabs A:link, DIV.tabs A:visited, 135 | DIV.tabs A:active, DIV.tabs A:hover 136 | { 137 | color: #1A419D; 138 | } 139 | 140 | DIV.tabs SPAN 141 | { 142 | float : left; 143 | display : block; 144 | background : url("tab_l.gif") no-repeat left top; 145 | padding : 5px 9px; 146 | white-space : nowrap; 147 | } 148 | 149 | DIV.tabs INPUT 150 | { 151 | float : right; 152 | display : inline; 153 | font-size : 1em; 154 | } 155 | 156 | DIV.tabs TD 157 | { 158 | font-size : x-small; 159 | font-weight : bold; 160 | text-decoration : none; 161 | } 162 | 163 | 164 | 165 | DIV.tabs SPAN {float : none;} 166 | 167 | DIV.tabs A:hover SPAN 168 | { 169 | background-position: 0% -150px; 170 | } 171 | 172 | DIV.tabs LI#current A 173 | { 174 | background-position: 100% -150px; 175 | border-width : 0px; 176 | } 177 | 178 | DIV.tabs LI#current SPAN 179 | { 180 | background-position: 0% -150px; 181 | padding-bottom : 6px; 182 | } 183 | 184 | DIV.nav 185 | { 186 | background : none; 187 | border : none; 188 | border-bottom : 1px solid #84B0C7; 189 | } 190 | 191 | DIV.groupHeader 192 | { 193 | padding-top: 30px; 194 | padding-bottom: 20px; 195 | background : none; 196 | border : none; 197 | border-bottom : 1px solid #84B0C7; 198 | font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif; 199 | font-variant: small-caps; 200 | font-size: 14pt; 201 | color:#0066CC; 202 | } 203 | 204 | .directory p 205 | { 206 | margin: 0px; 207 | white-space: nowrap; 208 | font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif; 209 | font-size: 10pt; 210 | font-weight: normal; 211 | } 212 | 213 | 214 | .directory h3 215 | { 216 | font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif; 217 | margin: 0px; 218 | margin-top: 1em; 219 | padding-bottom: 20px; 220 | font-size: 12pt; 221 | font-variant: small-caps; 222 | text-align: center; 223 | } 224 | 225 | .directory a:visited { 226 | font-weight: bold; 227 | text-decoration: none; 228 | color:#0066CC 229 | } 230 | 231 | -------------------------------------------------------------------------------- /src/Algebra.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __SOTH_ALGEBRA__ 2 | #define __SOTH_ALGEBRA__ 3 | 4 | #include 5 | #include 6 | #include "soth/api.hpp" 7 | 8 | /* -------------------------------------------------------------------------- */ 9 | /* -------------------------------------------------------------------------- */ 10 | /* -------------------------------------------------------------------------- */ 11 | 12 | namespace soth { 13 | // USING_PART_OF_NAMESPACE_EIGEN; 14 | using namespace Eigen; 15 | 16 | } // namespace soth 17 | 18 | /* -------------------------------------------------------------------------- */ 19 | /* --- DEBUG ---------------------------------------------------------------- */ 20 | /* -------------------------------------------------------------------------- */ 21 | 22 | namespace soth { 23 | #define EI_FOREACH(a, b) for (Index a = 0; a < b.size(); ++a) 24 | 25 | struct MATLAB { 26 | SOTH_EXPORT friend std::ostream& operator<<(std::ostream& os, 27 | const MATLAB& m); 28 | 29 | SOTH_EXPORT MATLAB(const double& x); 30 | template 31 | MATLAB(const MatrixBase& m1) { 32 | genericInit(m1); 33 | } 34 | template 35 | MATLAB(const MatrixBase& m1, bool id); 36 | template 37 | MATLAB(const MatrixXd::Index size, const Rotation& m1); 38 | 39 | template 40 | void genericInit(const MatrixBase& m1); 41 | 42 | template 43 | void initMatrix(const MatrixBase& m1); 44 | template 45 | void initVector(const VectorGen& v); 46 | SOTH_EXPORT inline void initMatrixNull(void); 47 | SOTH_EXPORT inline void initMatrixColNull(unsigned int size); 48 | SOTH_EXPORT inline void initMatrixRowNull(unsigned int size); 49 | 50 | SOTH_EXPORT static bool fullPrec; 51 | SOTH_EXPORT static unsigned int precision; 52 | std::string str; 53 | }; 54 | 55 | } // namespace soth 56 | 57 | // --- HEAVY CODE --- 58 | namespace soth { 59 | template 60 | void MATLAB::genericInit(const MatrixBase& m1) { 61 | if (m1.rows() == 0) 62 | initMatrixRowNull((unsigned int)m1.cols()); 63 | else if (m1.cols() == 0) 64 | initMatrixColNull((unsigned int)m1.rows()); 65 | else if (m1.IsVectorAtCompileTime) { 66 | if (m1.cols() == 1) 67 | initVector(m1.col(0)); 68 | else if (m1.rows() == 1) 69 | initVector(m1.row(0)); 70 | } else 71 | initMatrix(m1); 72 | } 73 | 74 | template 75 | void MATLAB::initVector(const VectorGen& m1) { 76 | std::ostringstream os; 77 | os.precision(MATLAB::precision); 78 | os << "[ "; 79 | std::ostringstream ostmp; 80 | ostmp.precision(MATLAB::precision); 81 | for (int i = 0; i < m1.size(); ++i) { 82 | if (m1[i] < 0) 83 | ostmp << "-"; 84 | else 85 | ostmp << " "; 86 | if (MATLAB::fullPrec || (std::abs(m1[i]) > 1e-6)) 87 | ostmp << std::abs(m1[i]); 88 | else { 89 | ostmp << "0"; 90 | } 91 | if (m1.size() != i + 1) { 92 | ostmp << ","; 93 | const int size = (int)ostmp.str().length(); 94 | for (unsigned int i = size; i < 8; ++i) ostmp << " "; 95 | ostmp << "\t"; 96 | } 97 | os << ostmp.str(); 98 | ostmp.str(""); 99 | } 100 | os << " ]';"; 101 | str = os.str(); 102 | } 103 | 104 | template 105 | MATLAB::MATLAB(const MatrixBase& m1, bool id) { 106 | if (id) { 107 | std::ostringstream os; 108 | os << "eye(" << m1.rows() << "," << m1.cols() << ");"; 109 | str = os.str(); 110 | } else { 111 | genericInit(m1); 112 | } 113 | } 114 | 115 | void MATLAB::initMatrixNull(void) { str = "[];"; } 116 | void MATLAB::initMatrixColNull(unsigned int size) { 117 | std::ostringstream os; 118 | os << "zeros(" << size << ",0);"; 119 | str = os.str(); 120 | } 121 | void MATLAB::initMatrixRowNull(unsigned int size) { 122 | std::ostringstream os; 123 | os << "zeros(0," << size << ");"; 124 | str = os.str(); 125 | } 126 | 127 | template 128 | void MATLAB::initMatrix(const MatrixBase& m1) { 129 | std::ostringstream os; 130 | os.precision(MATLAB::precision); 131 | if (fullPrec) { 132 | os << "[...\n" << m1 << "];"; 133 | str = os.str(); 134 | return; 135 | } 136 | os << "...\n[ "; 137 | std::ostringstream ostmp; 138 | ostmp.precision(MATLAB::precision); 139 | for (int i = 0; i < m1.rows(); ++i) { 140 | for (int j = 0; j < m1.cols(); ++j) { 141 | if (m1(i, j) < 0) 142 | ostmp << "-"; 143 | else 144 | ostmp << " "; 145 | if (MATLAB::fullPrec || (std::abs(m1(i, j)) > 1e-6)) 146 | ostmp << std::abs(m1(i, j)); 147 | else { 148 | ostmp << "0"; 149 | } 150 | if (m1.cols() != j + 1) { 151 | ostmp << ","; 152 | const int size = (int)ostmp.str().length(); 153 | for (unsigned int i = size; i < 8; ++i) ostmp << " "; 154 | ostmp << "\t"; 155 | } 156 | os << ostmp.str(); 157 | ostmp.str(""); 158 | } 159 | if (m1.rows() != i + 1) { 160 | os << " ;" << std::endl << " "; 161 | } else { 162 | os << " ];"; 163 | } 164 | } 165 | str = os.str(); 166 | } 167 | 168 | } // namespace soth 169 | 170 | #endif // #ifndef __SOTH_ALGEBRA__ 171 | -------------------------------------------------------------------------------- /unitTesting/tgivens.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Simple test of the givens class, with assertion. 4 | * 5 | * -------------------------------------------------------------------------- */ 6 | 7 | #define SOTH_DEBUG 8 | #define SOTH_DEBUG_MODE 50 9 | 10 | #include "soth/debug.hpp" 11 | 12 | #include 13 | #include "soth/Algebra.hpp" 14 | #include "soth/Givens.hpp" 15 | 16 | using namespace Eigen; 17 | using namespace soth; 18 | 19 | typedef Matrix IndexType; 20 | 21 | void testSimpleGivens() { 22 | using std::cout; 23 | using std::endl; 24 | 25 | MatrixXd M = MatrixXd::Random(4, 6); 26 | const double n0 = M.col(0).norm(); 27 | UNUSED(n0); 28 | sotDEBUG(1) << "M = " << (MATLAB)M << endl; 29 | 30 | sotDEBUG(1) << "Nullify col 0 ..."; 31 | Givens G1(M.col(0), 2, 3); 32 | sotDEBUG(1) << "G1 = " << G1.G.c() << "," << G1.G.s() << endl; 33 | G1.applyTransposeOnTheRight(M); 34 | // G1.applyTransposeOnTheLeft(M); 35 | sotDEBUG(1) << "G1'*M = " << (MATLAB)M << endl; 36 | 37 | Givens G2(M(1, 0), M(2, 0), 1, 2); 38 | G2.applyTransposeOnTheRight(M); 39 | sotDEBUG(1) << "G2'*G1'*M = " << (MATLAB)M << endl; 40 | 41 | Givens G3(M(0, 0), M(1, 0), 0, 1); 42 | G3.transpose() >> M; 43 | sotDEBUG(1) << "G3'*G2'*G1'*M = " << (MATLAB)M << endl; 44 | 45 | const double n1 = M.row(0).tail(M.cols() - 1).norm(); 46 | UNUSED(n1); 47 | sotDEBUG(1) << endl << "Nullify row 0 ..." << endl; 48 | for (int i = (int)M.cols() - 1; i > 1; --i) { 49 | // Givens G(M.row(0), i-1, i); 50 | M << Givens(M.row(0), i - 1, i); 51 | sotDEBUG(1) << "G1:3'*M*Prod(G)" << (MATLAB)M << endl; 52 | } 53 | 54 | cout << "Checking the application on the cols of MatrixXd ... " << std::flush; 55 | assert(M.col(0).tail(M.rows() - 1).norm() < 1e-6); 56 | assert(std::abs(M.col(0).norm() - n0) < 1e-6); 57 | cout << " ... OK! " << endl; 58 | cout << "Checking the application on the rows of MatrixXd ... " << std::flush; 59 | assert(M.row(0).tail(M.rows() - 2).norm() < 1e-6); 60 | assert(std::abs(M.row(0).tail(M.cols() - 1).norm() - n1) < 1e-6); 61 | cout << " ... OK! " << endl; 62 | } 63 | 64 | #include "soth/SubMatrix.hpp" 65 | void testSubMatrixGivens() { 66 | using std::cout; 67 | using std::endl; 68 | 69 | MatrixXd A = MatrixXd::Random(4, 6); 70 | sotDEBUG(1) << "A = " << (MATLAB)A << endl; 71 | 72 | IndexType idxr(4); 73 | idxr << 3, 1, 0, 2; 74 | IndexType idxc(6); 75 | idxc << 3, 5, 0, 1, 2, 4; 76 | SubMatrix M(A, idxr, idxc); 77 | sotDEBUG(1) << "M = " << (MATLAB)M << endl; 78 | 79 | const double n0 = M.col(0).norm(); 80 | UNUSED(n0); 81 | 82 | sotDEBUG(1) << "Nullify col 0 ..." << endl; 83 | Givens G1(M.col(0), 2, 3); 84 | G1.applyTransposeOnTheRight(M); 85 | sotDEBUG(1) << "G1'*M = " << (MATLAB)M << endl; 86 | 87 | Givens G2(M.col(0), 1, 2); 88 | G2.applyTransposeOnTheRight(M); 89 | sotDEBUG(1) << "G2'*G1'*M = " << (MATLAB)M << endl; 90 | 91 | Givens G3(M(0, 0), M(1, 0), 0, 1); 92 | G3.applyTransposeOnTheRight(M); 93 | sotDEBUG(1) << "G3'*G2'*G1'*M = " << (MATLAB)M << endl; 94 | 95 | const double n1 = M.row(0).tail(M.cols() - 1).norm(); 96 | UNUSED(n1); 97 | sotDEBUG(1) << endl << "Nullify row 0 ..." << endl; 98 | for (int i = (int)M.cols() - 1; i > 1; --i) { 99 | M << Givens(M.row(0), i - 1, i); 100 | // Givens (M.row(0), i-1, i).applyThisOnTheLeft(M); 101 | sotDEBUG(1) << "G1:3'*M*Prod(G)" << (MATLAB)M << endl; 102 | } 103 | 104 | cout << "Checking the application on the cols of a submatrix ... " 105 | << std::flush; 106 | assert(M.col(0).tail(M.rows() - 1).norm() < 1e-6); 107 | assert(std::abs(M.col(0).norm() - n0) < 1e-6); 108 | cout << " ... OK! " << endl; 109 | cout << "Checking the application on the rows of a submatrix ... " 110 | << std::flush; 111 | assert(M.row(0).tail(M.rows() - 2).norm() < 1e-6); 112 | assert(std::abs(M.row(0).tail(M.cols() - 1).norm() - n1) < 1e-6); 113 | cout << " ... OK! " << endl; 114 | } 115 | 116 | void testSequenceSub() { 117 | using std::cout; 118 | using std::endl; 119 | 120 | MatrixXd A = MatrixXd::Random(4, 6); 121 | sotDEBUG(1) << "A = " << (MATLAB)A << endl; 122 | 123 | IndexType idxr(4); 124 | idxr << 3, 1, 0, 2; 125 | IndexType idxc(6); 126 | idxc << 3, 5, 0, 1, 2, 4; 127 | SubMatrix M(A, idxr, idxc); 128 | sotDEBUG(1) << "M = " << (MATLAB)M << endl; 129 | MatrixXd Msav = M; 130 | GivensSequence U, V; 131 | 132 | Givens G1(M.col(0), 2, 3); 133 | G1.applyTransposeOnTheRight(M); 134 | U.push(G1); 135 | 136 | Givens G2(M.col(0), 1, 2); 137 | G2.applyTransposeOnTheRight(M); 138 | U.push(G2); 139 | 140 | Givens G3(M(0, 0), M(1, 0), 0, 1); 141 | G3.applyTransposeOnTheRight(M); 142 | U.push(G3); 143 | 144 | Givens Gr; 145 | for (int i = (int)M.cols() - 1; i > 1; --i) { 146 | Gr.makeGivens(M.row(0), i - 1, i); 147 | Gr.applyThisOnTheLeft(M); 148 | V.push(Gr); 149 | } 150 | 151 | U.transpose() >> Msav; 152 | Msav << V; 153 | sotDEBUG(1) << "Msav = " << (MATLAB)Msav << endl; 154 | 155 | cout << "Checking the sequence of givens ... " << std::flush; 156 | assert((M - Msav).norm() < 1e-6); 157 | cout << " ... OK! " << endl; 158 | } 159 | 160 | int main() { 161 | sotDebugTrace::openFile(); 162 | testSimpleGivens(); 163 | testSubMatrixGivens(); 164 | testSequenceSub(); 165 | } 166 | -------------------------------------------------------------------------------- /unitTesting/COD.hpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Compute the COD A = U [ L 0 ] V 4 | * [ 0 0 ] 5 | * and provide the ls solver x = A^+ b. 6 | * 7 | * -------------------------------------------------------------------------- */ 8 | 9 | #ifndef __SOTH_COD__ 10 | #define __SOTH_COD__ 11 | 12 | #include 13 | #include "soth/Algebra.hpp" 14 | #include "soth/Givens.hpp" 15 | #include "soth/debug.hpp" 16 | 17 | namespace soth { 18 | /* --------------------------------------------------------------------------- 19 | */ 20 | /* --- COD SOLVER ------------------------------------------------------------ 21 | */ 22 | /* --------------------------------------------------------------------------- 23 | */ 24 | /* Compute the decomposition A=U.L.V', with U and L rotation matrices, and 25 | * L = [ L0 0 ; 0 0 ] with L0 triangular inf, with non zero diagonal. 26 | * Use this decompo to solve min||Ax-b||. 27 | */ 28 | struct COD { 29 | MatrixXd L, Ainit; 30 | ColPivHouseholderQR qrv; 31 | int rank, NC, NR; 32 | MatrixXd U, V; 33 | bool m_computeFullU, m_computeThinU, m_computeFullV, m_computeThinV; 34 | 35 | COD(unsigned int computationOptions = 0) { options(computationOptions); } 36 | void options(unsigned int computationOptions = 0) { 37 | m_computeFullU = (computationOptions & ComputeFullU) != 0; 38 | m_computeThinU = (computationOptions & ComputeThinU) != 0; 39 | m_computeFullV = (computationOptions & ComputeFullV) != 0; 40 | m_computeThinV = (computationOptions & ComputeThinV) != 0; 41 | } 42 | 43 | /* Nullify row k of L using the first rows as a lower triangle, supposing that 44 | * LX = [ L ; X ], L of size MxM and lower triangular and k>M. */ 45 | void leftGivens(const int k) { 46 | for (int j = rank - 1; j >= 0; --j) { 47 | Givens G1(L.col(j), j, k); 48 | G1.transpose() >> L; 49 | if (m_computeFullU || m_computeThinU) { 50 | U << G1; 51 | } 52 | sotDEBUG(5) << "LX" << j << " = " << (MATLAB)L << std::endl << std::endl; 53 | } 54 | } 55 | 56 | int computeRank(int rank, const double EPSILON = 1e-8) { 57 | if (rank > 0) return rank; 58 | 59 | const MatrixXd& QR = qrv.matrixQR(); 60 | const int R0 = std::min(NR, NC); 61 | for (rank = R0; rank > 0; --rank) { 62 | if (fabs(QR(rank - 1, rank - 1)) > EPSILON) break; 63 | } 64 | return rank; 65 | } 66 | 67 | void compute(const MatrixXd& A, const int rank_ = -1, 68 | const double EPSILON = 1e-8) { 69 | NC = (int)A.cols(); 70 | NR = (int)A.rows(); 71 | #ifdef DEBUG 72 | Ainit = A; 73 | #endif 74 | 75 | qrv.compute(A.transpose()); 76 | sotDEBUG(5) << "QR= " << (MATLAB)qrv.matrixQR() << std::endl; 77 | 78 | rank = computeRank(rank_, EPSILON); 79 | L = qrv.matrixQR().topRows(rank).triangularView().transpose(); 80 | sotDEBUG(5) << "L = " << (MATLAB)L << std::endl; 81 | 82 | if (m_computeFullV) { 83 | V.setIdentity(NC, NC); 84 | V.applyOnTheRight(qrv.householderQ()); 85 | sotDEBUG(5) << "V = " << (MATLAB)V << std::endl; 86 | } else if (m_computeThinV) { 87 | V.resize(NC, rank); 88 | V.setIdentity(NC, rank); 89 | V.applyOnTheLeft(qrv.householderQ()); 90 | } 91 | 92 | if (m_computeFullU || m_computeThinU) { 93 | U = qrv.colsPermutation(); 94 | sotDEBUG(5) << "Pi = " << (MATLAB)U << std::endl; 95 | } 96 | 97 | for (int k = rank; k < NR; ++k) leftGivens(k); 98 | 99 | sotDEBUG(5) << "L = " << (MATLAB)L << std::endl; 100 | sotDEBUG(5) << "U = " << (MATLAB)U << std::endl; 101 | } 102 | 103 | /* Compute M:=M*V */ 104 | template 105 | void applyMatrixV(MatrixBase& M) { 106 | M.applyOnTheRight(qrv.householderQ()); 107 | } 108 | 109 | MatrixXd& matrixU() { return U; } 110 | MatrixXd& matrixV() { return V; } 111 | MatrixXd::ColsBlockXpr matrixUr() { return U.leftCols(rank); } 112 | MatrixXd::ColsBlockXpr matrixVr() { return V.leftCols(rank); } 113 | MatrixXd::ColsBlockXpr matrixUo() { return U.rightCols(NR - rank); } 114 | MatrixXd::ColsBlockXpr matrixVo() { return V.rightCols(NC - rank); } 115 | #ifndef XENIAL_DETECTED 116 | TriangularView, Lower> matrixL() { return L.topRows(rank); } 117 | #else 118 | TriangularView, Lower> matrixL() { 119 | return L.topRows(rank).triangularView(); 120 | } 121 | #endif 122 | 123 | /* Solve min||Ax-b|| for a matrix A whose rank is given. */ 124 | VectorXd solve(const VectorXd& b, bool inY = false) { 125 | if (rank == 0) return VectorXd::Zero(NC); 126 | 127 | /* Approximate solution using no basis transfo (result is meanigless 128 | * appart from the computation time pov. */ 129 | /* 130 | VectorXd sol = b.head(rank); 131 | matrixL().solveInPlace( sol ); 132 | VectorXd res; res.setZero(NC); 133 | res.head(rank)=sol; return res; 134 | */ 135 | 136 | /* With plain matrices. */ 137 | /* 138 | VectorXd sol = matrixUr().transpose()*b; 139 | matrixL().solveInPlace( sol ); 140 | return matrixVr()*sol; 141 | */ 142 | 143 | /* Using the HH representation of V. */ 144 | assert(m_computeThinU || m_computeFullU); 145 | VectorXd sol; 146 | if (inY) 147 | sol.setZero(rank); 148 | else 149 | sol.setZero(NC); 150 | sol.head(rank) = matrixUr().transpose() * b; 151 | matrixL().solveInPlace(sol.head(rank)); 152 | if (!inY) sol.applyOnTheLeft(qrv.householderQ()); 153 | return sol; 154 | } 155 | }; 156 | 157 | } // namespace soth 158 | 159 | #endif // #ifndef __SOTH_COD__ 160 | -------------------------------------------------------------------------------- /src/BaseY.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __SOTH_BASEY__ 2 | #define __SOTH_BASEY__ 3 | 4 | #include 5 | #include "soth/Algebra.hpp" 6 | #include "soth/Givens.hpp" 7 | #include "soth/api.hpp" 8 | 9 | namespace soth { 10 | class SOTH_EXPORT BaseY { 11 | protected: 12 | typedef MatrixXd::Index Index; 13 | typedef Diagonal HCoeffsType; 14 | typedef HouseholderSequence HouseholderSequenceType; 15 | typedef Diagonal HCoeffsType_const; 16 | typedef HouseholderSequence 17 | HouseholderSequenceType_const; 18 | 19 | protected: 20 | public: 21 | bool isExplicit; 22 | Index size; 23 | Index rank; 24 | MatrixXd matrixExplicit; 25 | MatrixXd householderEssential; 26 | 27 | public: 28 | // Empty construction with memory allocation. 29 | BaseY(const Index& size); 30 | 31 | void computeExplicitly(); 32 | void reset() { 33 | rank = 0; 34 | #ifndef NDEBUG 35 | matrixExplicit.setZero(); 36 | #endif 37 | isExplicit = false; 38 | } 39 | 40 | public: 41 | /* --- Accessor --- */ 42 | MatrixXd& getHouseholderEssential() { return householderEssential; } 43 | const MatrixXd& getHouseholderEssential() const { 44 | return householderEssential; 45 | } 46 | 47 | Block getNextHouseholderEssential() { 48 | return householderEssential.bottomRightCorner(size - rank, size - rank); 49 | } 50 | 51 | void updateRank(Index r) { rank = r; } 52 | void increaseRank(Index r) { rank += r; } 53 | inline Index getRank(void) const { return rank; } 54 | 55 | HouseholderSequenceType_const getHouseholderSequence() const { 56 | HouseholderSequenceType_const res = HouseholderSequenceType_const( 57 | householderEssential, householderEssential.diagonal()); 58 | //,trans=false,actualVectors=rank,shift=0 59 | res.setLength(rank); 60 | return res; 61 | } 62 | 63 | /* --- Multiplier --- */ 64 | /* TODO: when not explicit, it is cheaper to work directly on the 65 | * result memory, while it is the opposit when explicit. What 66 | * should we do? 67 | */ 68 | 69 | // v := Y*v = v*Y'. 70 | template 71 | void applyThisOnVector(VectorGen& v) const { 72 | applyThisOnTheRight(v); 73 | } 74 | // v := Y'*v = v*Y. 75 | template 76 | void applyTransposeOnVector(VectorGen& v) const { 77 | applyThisOnTheLeft(v); 78 | } 79 | 80 | // M := M*Y. 81 | template 82 | void applyThisOnTheLeft(MatrixBase& M) const; 83 | // M := M*Y'. 84 | template 85 | void applyTransposeOnTheLeft(MatrixBase& M) const; 86 | // M := Y*M. 87 | template 88 | void applyThisOnTheRight(MatrixBase& M) const; 89 | // M := Y'*M. 90 | template 91 | void applyTransposeOnTheRight(MatrixBase& M) const; 92 | 93 | BaseY& operator*=(const Givens& g); 94 | BaseY& operator*=(const GivensSequence& G); 95 | 96 | template 97 | void multiply(const MatrixBase& m, 98 | MatrixBase& res) const { 99 | if (isExplicit) 100 | res = matrixExplicit * m; 101 | else { 102 | res = m; 103 | applyThisOnTheRight(res); 104 | } 105 | } 106 | template 107 | void transposeMultiply(const MatrixBase& m, 108 | MatrixBase& res) const { 109 | if (isExplicit) 110 | res = matrixExplicit.transpose() * m; 111 | else { 112 | res = m; 113 | applyTransposeOnTheRight(res); 114 | } 115 | } 116 | }; 117 | 118 | /* --- HEAVY CODE --------------------------------------------------------- */ 119 | /* --- HEAVY CODE --------------------------------------------------------- */ 120 | /* --- HEAVY CODE --------------------------------------------------------- */ 121 | 122 | // // v := Y*v = v*Y'. 123 | // template< typename VectorGen > 124 | // void BaseY:: 125 | // applyThisOnVector( VectorGen & v ) const 126 | // { 127 | // if( isExplicit ) 128 | // { 129 | ///*TODO*/ throw "TODO"; 130 | // } 131 | // else 132 | // { 133 | // matrixHH.applyThisOnVector(v); 134 | // } 135 | // } 136 | 137 | // // v := Y'*v = v*Y. 138 | // template< typename VectorGen > 139 | // void BaseY:: 140 | // applyTransposeOnVector( VectorGen & v ) const 141 | // { 142 | // if( isExplicit ) 143 | // { 144 | ///*TODO*/ throw "TODO"; 145 | // } 146 | // else 147 | // { 148 | // matrixHH.applyTransposeOnVector(v); 149 | // } 150 | // } 151 | 152 | // M := M*Y. 153 | template 154 | void BaseY::applyThisOnTheLeft(MatrixBase& M) const { 155 | if (isExplicit) { 156 | MatrixXd tmp = M; 157 | M = tmp * matrixExplicit; 158 | } else { 159 | M.applyOnTheRight(getHouseholderSequence()); 160 | } 161 | } 162 | 163 | // M := M*Y'. 164 | template 165 | void BaseY::applyTransposeOnTheLeft(MatrixBase& M) const { 166 | if (isExplicit) { 167 | M = M * matrixExplicit.transpose(); 168 | } else { 169 | M.applyOnTheRight(getHouseholderSequence().transpose()); 170 | } 171 | } 172 | 173 | // M := Y*M. 174 | template 175 | void BaseY::applyThisOnTheRight(MatrixBase& M) const { 176 | if (isExplicit) { 177 | M = matrixExplicit * M; 178 | } else { 179 | M.applyOnTheLeft(getHouseholderSequence()); 180 | } 181 | } 182 | 183 | // M := Y'*M. 184 | template 185 | void BaseY::applyTransposeOnTheRight(MatrixBase& M) const { 186 | if (isExplicit) { 187 | M = matrixExplicit.transpose() * M; 188 | } else { 189 | M.applyOnTheLeft(getHouseholderSequence().transpose()); 190 | } 191 | } 192 | 193 | } // namespace soth 194 | 195 | #endif //#ifndef __SOTH_BASEY__ 196 | -------------------------------------------------------------------------------- /src/Bound.cpp: -------------------------------------------------------------------------------- 1 | #include "soth/Bound.hpp" 2 | #include 3 | #include 4 | 5 | namespace soth { 6 | Bound::Bound(void) : type(BOUND_NONE), valInf(0), valSup(0), valTwin(valInf) {} 7 | 8 | Bound::Bound(const Bound& clone) 9 | : type(clone.type), 10 | valInf(clone.valInf), 11 | valSup(clone.valSup), 12 | valTwin(valInf) {} 13 | 14 | Bound::Bound(const double& val, bound_t inType) 15 | : type(inType), valInf(val), valSup(val), valTwin(valInf) { 16 | assert(inType != BOUND_DOUBLE); 17 | assert(inType != BOUND_NONE); 18 | 19 | type = inType; 20 | } 21 | 22 | Bound::Bound(const double& inValInf, const double& inValSup) 23 | : type(BOUND_DOUBLE), valInf(inValInf), valSup(inValSup), valTwin(valInf) {} 24 | Bound::Bound(const double& val) 25 | : type(BOUND_TWIN), valInf(val), valSup(val), valTwin(valInf) {} 26 | 27 | const double& Bound::getBound(bound_t inType) const { 28 | switch (inType) { 29 | case BOUND_INF: 30 | assert((type == BOUND_INF) || (type == BOUND_DOUBLE)); 31 | return valInf; 32 | case BOUND_SUP: 33 | assert((type == BOUND_SUP) || (type == BOUND_DOUBLE)); 34 | return valSup; 35 | case BOUND_TWIN: 36 | assert(type == BOUND_TWIN); 37 | return valTwin; 38 | case BOUND_DOUBLE: 39 | case BOUND_NONE:; 40 | } 41 | assert(false && "Cannot get a bound for 0 or double."); 42 | return valTwin; 43 | } 44 | 45 | /* Return the bound that is violated, NONE if bound are OK. 46 | * In case of twin-bounds, no check is performed, NONE is always returned. */ 47 | Bound::bound_t Bound::check(const double& val, const double& EPSILON) const { 48 | switch (type) { 49 | case BOUND_INF: 50 | if (val < valInf - EPSILON) return BOUND_INF; 51 | break; 52 | case BOUND_SUP: 53 | if (valSup + EPSILON < val) return BOUND_SUP; 54 | break; 55 | case BOUND_TWIN: 56 | break; 57 | case BOUND_DOUBLE: 58 | if (val < valInf - EPSILON) return BOUND_INF; 59 | if (valSup + EPSILON < val) return BOUND_SUP; 60 | break; 61 | case BOUND_NONE: 62 | assert(false && "Cannot check a bound for 0 constraint."); 63 | } 64 | return BOUND_NONE; 65 | } 66 | 67 | /* Return the bound that is violated, NONE if bound are OK. 68 | * In case of twin-bounds, no check is performed, NONE is always returned. */ 69 | Bound::bound_t Bound::check(const double& val, std::pair damp, 70 | const double& EPSILON) const { 71 | switch (type) { 72 | case BOUND_INF: 73 | if (val < valInf - damp.first - EPSILON) return BOUND_INF; 74 | break; 75 | case BOUND_SUP: 76 | if (valSup + EPSILON + damp.second < val) return BOUND_SUP; 77 | break; 78 | case BOUND_TWIN: 79 | break; 80 | case BOUND_DOUBLE: 81 | if (val < valInf - damp.first - EPSILON) return BOUND_INF; 82 | if (valSup + damp.first + EPSILON < val) return BOUND_SUP; 83 | break; 84 | case BOUND_NONE: 85 | assert(false && "Cannot check a bound for 0 constraint."); 86 | } 87 | return BOUND_NONE; 88 | } 89 | 90 | /* Return the bound that is violated, NONE if bound are OK. 91 | * In case of twin-bounds, no check is performed, NONE is always returned. */ 92 | Bound::bound_t Bound::checkSaturation(const double& val, 93 | const double& EPSILON) const { 94 | switch (type) { 95 | case BOUND_INF: 96 | if (std::abs(val - valInf) < EPSILON) return BOUND_INF; 97 | break; 98 | case BOUND_SUP: 99 | if (std::abs(val - valSup) < EPSILON) return BOUND_SUP; 100 | break; 101 | case BOUND_TWIN: 102 | break; 103 | case BOUND_DOUBLE: 104 | if (std::abs(val - valInf) < EPSILON) return BOUND_INF; 105 | if (std::abs(val - valSup) < EPSILON) return BOUND_SUP; 106 | break; 107 | case BOUND_NONE: 108 | assert(type != BOUND_NONE); 109 | } 110 | return BOUND_NONE; 111 | } 112 | 113 | double Bound::distance(const double& val) const { 114 | double res = -1; 115 | switch (type) { 116 | case BOUND_INF: 117 | if (val > valInf) 118 | res = 0; 119 | else 120 | res = valInf - val; 121 | break; 122 | case BOUND_SUP: 123 | if (val < valSup) 124 | res = 0; 125 | else 126 | res = val - valSup; 127 | break; 128 | case BOUND_TWIN: 129 | res = std::abs(val - valTwin); 130 | break; 131 | case BOUND_DOUBLE: 132 | if (val > valInf) 133 | if (val < valSup) 134 | res = 0; 135 | else 136 | res = val - valSup; 137 | else 138 | res = valInf - val; 139 | break; 140 | case BOUND_NONE: 141 | assert(type != BOUND_NONE); 142 | break; 143 | } 144 | assert(res >= 0); 145 | return res; 146 | } 147 | 148 | Bound& Bound::operator=(const Bound& clone) { 149 | valInf = clone.valInf; 150 | valSup = clone.valSup; 151 | type = clone.type; 152 | return *this; 153 | } 154 | 155 | Bound& Bound::operator=(const double& val) { 156 | valTwin = val; 157 | type = BOUND_TWIN; 158 | return *this; 159 | } 160 | 161 | Bound& Bound::operator=(const std::pair& val) { 162 | valInf = val.first; 163 | valSup = val.second; 164 | type = BOUND_DOUBLE; 165 | return *this; 166 | } 167 | 168 | std::ostream& operator<<(std::ostream& os, const Bound& b) { 169 | switch (b.type) { 170 | case Bound::BOUND_INF: 171 | os << "[ " << b.valInf << ", +inf ]"; 172 | break; 173 | case Bound::BOUND_SUP: 174 | os << "[ -inf ," << b.valSup << " ]"; 175 | break; 176 | case Bound::BOUND_TWIN: 177 | os << "[ " << b.valTwin << ", " << b.valTwin << " ]"; 178 | break; 179 | case Bound::BOUND_DOUBLE: 180 | os << "[ " << b.valInf << ", " << b.valSup << " ]"; 181 | break; 182 | case Bound::BOUND_NONE: 183 | os << "[ -inf, +inf ]"; 184 | } 185 | return os; 186 | } 187 | // std::ostream& operator<< (std::ostream& os, const bound_vector_t& t) 188 | // { 189 | // os << "[ "; 190 | // for( bound_vector_t::const_iterator iter=t.begin(); 191 | // iter!=t.end();++iter ) 192 | // { 193 | // os << *iter; 194 | // if( iter+1 != t.end() ) os <<"; "; else os<<" ]"; 195 | // } 196 | // return os; 197 | // } 198 | 199 | std::ostream& operator<<(std::ostream& os, const ConstraintRef& cst) { 200 | switch (cst.type) { 201 | case Bound::BOUND_INF: 202 | os << "-"; 203 | break; 204 | case Bound::BOUND_SUP: 205 | os << "+"; 206 | break; 207 | case Bound::BOUND_DOUBLE: 208 | os << "+/-"; 209 | break; 210 | case Bound::BOUND_TWIN: 211 | os << "="; 212 | break; 213 | case Bound::BOUND_NONE: 214 | os << "(o)"; 215 | break; 216 | } 217 | return os << cst.row; 218 | } 219 | 220 | const ConstraintRef CONSTRAINT_VOID; //(-1,Bound::BOUND_NONE); 221 | 222 | } // namespace soth 223 | -------------------------------------------------------------------------------- /unitTesting/tchrono.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Chrono test of the active search. Should be properly rewritten. 4 | * 5 | * -------------------------------------------------------------------------- */ 6 | 7 | //#define SOTH_DEBUG 8 | //#define SOTH_DEBUG_MODE 45 9 | #include "soth/HCOD.hpp" 10 | #include "soth/Random.hpp" 11 | #include "soth/debug.hpp" 12 | 13 | #ifndef WIN32 14 | #include 15 | #endif // WIN32 16 | 17 | #include 18 | #include "RandomGenerator.hpp" 19 | #include "gettimeofday.hpp" 20 | #include "soth/DestructiveColPivQR.hpp" 21 | 22 | using namespace soth; 23 | using std::endl; 24 | 25 | /* -------------------------------------------------------------------------- */ 26 | namespace soth { 27 | struct Now { 28 | long int sec, usec; 29 | Now(void) { 30 | struct timeval tref; 31 | gettimeofday(&tref, NULL); 32 | sec = tref.tv_sec; 33 | usec = tref.tv_usec; 34 | } 35 | }; 36 | double operator-(const Now& t1, const Now& t0) { 37 | return ((double)(t1.sec - t0.sec)) * 1000.0 + 38 | ((double)(t1.usec - t0.usec) + 0.0) / 1000.0; 39 | } 40 | std::ostream& operator<<(std::ostream& os, const Now& now) { 41 | return os << now.sec << "' " << now.usec; 42 | } 43 | 44 | // std::ostream & chronout = std::cout; 45 | 46 | // class chrono_context_ptr_t; 47 | // struct Chrono 48 | // { 49 | // chrono_context_ptr_t context; 50 | // std::string definition; 51 | // Chrono( chrono_context_ptr_t context,std::string definition ) 52 | // : context(context),definition(definition) 53 | // {} 54 | // ~Chrono() 55 | // { 56 | // chronout << definition << ": " << (Now()-context->t0) << std::endl; 57 | // } 58 | // }; 59 | // boost::smart_ptr chrono_ptr_t; 60 | 61 | // struct ChronoContext 62 | // { 63 | // time_t t0; 64 | // std::list chronoList; 65 | // }; 66 | // std::map< std::string,ChronoContext > 67 | 68 | } // namespace soth 69 | 70 | /* -------------------------------------------------------------------------- */ 71 | /* -------------------------------------------------------------------------- */ 72 | /* -------------------------------------------------------------------------- */ 73 | int main(int argc, char** argv) { 74 | #ifndef NDEBUG 75 | sotDebugTrace::openFile(); 76 | #endif 77 | 78 | unsigned int NB_STAGE, NC; 79 | std::vector NR, RANKLINKED, RANKFREE; 80 | std::vector J; 81 | std::vector b; 82 | 83 | if ((argc == 3) && std::string(argv[1]) == "-file") { 84 | readProblemFromFile(argv[2], J, b, NB_STAGE, NR, NC); 85 | } else { 86 | /* Initialize the seed. */ 87 | struct timeval tv; 88 | gettimeofday(&tv, NULL); 89 | int seed = (int)(tv.tv_usec % 7919); //= 7594; 90 | if (argc == 2) { 91 | seed = atoi(argv[1]); 92 | } 93 | std::cout << "seed = " << seed << std::endl; 94 | soth::Random::setSeed(seed); 95 | 96 | /* Decide the size of the problem. */ 97 | generateRandomProfile(NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 98 | /* Initialize J and b. */ 99 | generateDeficientDataSet(J, b, NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 100 | } 101 | 102 | std::cout << "NB_STAGE=" << NB_STAGE << ", NC=" << NC << endl; 103 | std::cout << endl; 104 | for (unsigned int i = 0; i < NB_STAGE; ++i) { 105 | std::cout << "J" << i + 1 << " = " << (soth::MATLAB)J[i] << std::endl; 106 | std::cout << "e" << i + 1 << " = " << b[i] << ";" << std::endl; 107 | } 108 | 109 | // if( seed==317) assert( std::abs(J[0](0,0)-(-1.1149))<1e-5 ); 110 | 111 | /* SOTH structure construction. */ 112 | soth::HCOD hcod(NC, NB_STAGE); 113 | for (unsigned int i = 0; i < NB_STAGE; ++i) { 114 | hcod.pushBackStage(J[i], b[i]); 115 | } 116 | hcod.setNameByOrder("stage_"); 117 | 118 | /* --- TESTS -------------------------------------------------------------- */ 119 | #ifdef NDEBUG 120 | const unsigned int nbTest = 1000; 121 | #else 122 | const unsigned int nbTest = 10; 123 | #endif 124 | 125 | VectorXd solution; 126 | 127 | Now t0; 128 | for (unsigned int i = 0; i < nbTest; ++i) { 129 | hcod.initialize(); 130 | } 131 | 132 | Now t1; 133 | for (unsigned int i = 0; i < nbTest; ++i) hcod.Y.computeExplicitly(); 134 | 135 | Now t2; 136 | for (unsigned int i = 0; i < nbTest; ++i) hcod.computeSolution(true); 137 | 138 | Now t3; 139 | // for( unsigned int i=0;i mQR(A, Y, 1e-6); 178 | Now tf; 179 | std::cout << "QR " << Nctest << "x" << Nrtest << " = " << tf - t3 << "us" 180 | << endl; 181 | } 182 | 183 | { // simple Linv test 184 | const int Nctest = NC; 185 | const int Nrtest = std::min(NC, (unsigned int)21); 186 | MatrixXd A = MatrixXd::Random(Nrtest, Nctest); 187 | VectorXd b(Nrtest); 188 | 189 | Now t3; 190 | for (unsigned int i = 0; i < nbTest; ++i) 191 | solveInPlaceWithLowerTriangular(A.leftCols(Nrtest), b); 192 | Now tf; 193 | std::cout << "Linv " << Nrtest << "x" << Nrtest << " = " << tf - t3 << "us" 194 | << endl; 195 | } 196 | 197 | { // Double test with matrix multiplication, just to be sure. 198 | const int Nctest = NC; 199 | const int Nrtest = 21; 200 | MatrixXd A = MatrixXd::Random(Nrtest, Nctest); 201 | MatrixXd B = MatrixXd::Random(Nctest, Nctest); 202 | 203 | MatrixXd C(Nrtest, Nctest); 204 | Now t3; 205 | for (unsigned int i = 0; i < nbTest; ++i) C = A * B; 206 | Now tf; 207 | std::cout << Nctest << "x" << Nrtest << " = " << tf - t3 << "us" << endl; 208 | std::cout << C(0, 0); 209 | } 210 | } 211 | -------------------------------------------------------------------------------- /COPYING: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /src/ActiveSet.cpp: -------------------------------------------------------------------------------- 1 | #include "soth/ActiveSet.hpp" 2 | 3 | namespace soth { 4 | /* ------------------------------------------------------------------------ */ 5 | /* --- CONSTRUCTION ------------------------------------------------------- */ 6 | /* ------------------------------------------------------------------------ */ 7 | 8 | ActiveSet::ActiveSet(unsigned int nr) 9 | : cstMap(nr), 10 | cstMapInv(nr), 11 | freerow(nr), 12 | freezed(nr), 13 | activated(nr), 14 | dampingInf(nr), 15 | dampingSup(nr), 16 | nba(0) { 17 | reset(); 18 | } 19 | 20 | void ActiveSet::reset(void) { 21 | std::fill(cstMap.begin(), cstMap.end(), CONSTRAINT_VOID); 22 | std::fill(cstMapInv.begin(), cstMapInv.end(), size()); 23 | std::fill(freerow.begin(), freerow.end(), true); 24 | std::fill(freezed.begin(), freezed.end(), false); 25 | activated.fill(Bound::BOUND_NONE); 26 | dampingInf.setZero(); 27 | dampingSup.setZero(); 28 | 29 | nba = 0; 30 | } 31 | 32 | /* ------------------------------------------------------------------------ */ 33 | /* --- ACTIVE ------------------------------------------------------------- */ 34 | /* ------------------------------------------------------------------------ */ 35 | 36 | /* Active the constraint (ie in J(ref,:)) at line (ie in ML(row,:)) 37 | * with bound type. */ 38 | void ActiveSet::active(unsigned int ref, Bound::bound_t type, 39 | unsigned int row) { 40 | assert(ref < size()); 41 | assert((cstMap[ref].type == Bound::BOUND_NONE) && 42 | "Constraint has not been properly unactivated."); 43 | assert(row < size()); 44 | assert(freerow[row]); 45 | assert(cstMapInv[row] == size()); 46 | assert(type != Bound::BOUND_NONE); 47 | 48 | cstMap[ref] = ConstraintRef(row, type); 49 | cstMapInv[row] = ref; 50 | freerow[row] = false; 51 | nba++; 52 | 53 | if (activated[ref] == Bound::BOUND_NONE) 54 | activated[ref] = type; 55 | else { 56 | // assert( activated[ref]!=type ); 57 | assert(type == Bound::BOUND_INF || type == Bound::BOUND_SUP); 58 | activated[ref] = Bound::BOUND_DOUBLE; 59 | } 60 | } 61 | 62 | /* Active the given constraint at any free row of ML, and return the 63 | * position of the selected free row. */ 64 | unsigned int ActiveSet::activeRow(unsigned int ref, Bound::bound_t type) { 65 | const unsigned int row = getAFreeRow(); 66 | assert(row < size()); 67 | active(ref, type, row); 68 | return row; 69 | } 70 | 71 | /* Unactive the constraint at line of ML and frees the corresponding line. 72 | */ 73 | void ActiveSet::unactiveRow(unsigned int row) { 74 | assert(row < size()); 75 | 76 | const unsigned int& cst = cstMapInv[row]; 77 | assert(cst < size()); 78 | assert(cstMap[cst].type != Bound::BOUND_NONE); 79 | 80 | cstMap[cst] = CONSTRAINT_VOID; 81 | freeARow(row); 82 | cstMapInv[row] = size(); 83 | nba--; 84 | } 85 | 86 | /* Pass the constraint to a twin mode. */ 87 | void ActiveSet::freeze(unsigned int ref) { 88 | assert(ref < size()); 89 | assert(cstMap[ref].type != Bound::BOUND_NONE); 90 | assert(cstMap[ref].type != Bound::BOUND_TWIN); 91 | assert(!freezed[ref]); 92 | 93 | freezed[ref] = true; 94 | } 95 | 96 | bool ActiveSet::isRowFree(unsigned int row) { return freerow[row]; } 97 | unsigned int ActiveSet::getAFreeRow(void) { 98 | /* TODO: it is possible to store the first freeline. */ 99 | assert(nba < size()); 100 | for (unsigned int row = 0; row < size(); ++row) { 101 | if (freerow[row]) return row; 102 | } 103 | assert(false && "Could never happen."); 104 | return -1; 105 | } 106 | void ActiveSet::freeARow(unsigned int row) { 107 | assert(!freerow[row]); 108 | freerow[row] = true; 109 | } 110 | 111 | void ActiveSet::dampBoundValue(const ConstraintRef& cst, const double& value) { 112 | assert(cst.type == Bound::BOUND_INF || cst.type == Bound::BOUND_SUP); 113 | const unsigned int& i = cst.row; 114 | if (cst.type == Bound::BOUND_INF && +value > dampingInf[i]) 115 | dampingInf[i] = +value; 116 | if (cst.type == Bound::BOUND_SUP && -value > dampingSup[i]) 117 | dampingSup[i] = -value; 118 | } 119 | 120 | std::pair ActiveSet::getBoundDamping(const unsigned int& cst) { 121 | return std::make_pair(dampingInf[cst], dampingSup[cst]); 122 | } 123 | 124 | /* ------------------------------------------------------------------------ */ 125 | /* --- ACCESSORS ---------------------------------------------------------- */ 126 | /* ------------------------------------------------------------------------ */ 127 | 128 | /* Give the reference of the constraint (ie line in J) located at row of 129 | * the working space ML. */ 130 | unsigned int ActiveSet::mapInv(unsigned int row) const { 131 | assert(row < size()); 132 | assert((cstMapInv[row] < size()) && "The requested row is not active"); 133 | return cstMapInv[row]; 134 | } 135 | unsigned int ActiveSet::map(unsigned int ref) const { 136 | assert(isActive(ref)); 137 | return cstMap[ref].row; 138 | } 139 | Bound::bound_t ActiveSet::whichBound(unsigned int ref, bool checkActive) const { 140 | assert(isActive(ref)); 141 | const Bound::bound_t& res = cstMap[ref].type; 142 | if (checkActive) { 143 | assert((res != Bound::BOUND_NONE) && (res != Bound::BOUND_DOUBLE)); 144 | } 145 | return res; 146 | } 147 | bool ActiveSet::isActive(unsigned int ref) const { 148 | assert(ref < size()); 149 | return (cstMap[ref].type != Bound::BOUND_NONE); 150 | } 151 | bool ActiveSet::wasActive(unsigned int ref, const Bound::bound_t type) const { 152 | assert(ref < size()); 153 | assert(activated.size() == (int)size()); 154 | assert(type == Bound::BOUND_INF || type == Bound::BOUND_SUP); 155 | assert(activated[ref] == Bound::BOUND_INF || 156 | activated[ref] == Bound::BOUND_SUP || 157 | activated[ref] == Bound::BOUND_NONE || 158 | activated[ref] == Bound::BOUND_DOUBLE); 159 | if (type == Bound::BOUND_INF) 160 | return activated[ref] == Bound::BOUND_INF || 161 | activated[ref] == Bound::BOUND_DOUBLE; 162 | else if (type == Bound::BOUND_SUP) 163 | return activated[ref] == Bound::BOUND_SUP || 164 | activated[ref] == Bound::BOUND_DOUBLE; 165 | else 166 | return (false); 167 | } 168 | double ActiveSet::sign(unsigned int ref) const { 169 | assert(isActive(ref)); 170 | assert(cstMap[ref].type != Bound::BOUND_DOUBLE); 171 | return (cstMap[ref].type == Bound::BOUND_INF) ? -1 : +1; 172 | } 173 | bool ActiveSet::isFreezed(unsigned int ref) const { 174 | assert(isActive(ref)); 175 | return (cstMap[ref].type == Bound::BOUND_TWIN) || (freezed[ref]); 176 | } 177 | /*: Return a compact of the active line, ordered by row values. */ 178 | VectorXi ActiveSet::getIndirection(void) const { 179 | if (nba == 0) return VectorXi(); 180 | 181 | VectorXi res(nba); 182 | int row = 0; 183 | for (unsigned int i = 0; i < size(); ++i) 184 | if (!freerow[i]) res(row++) = whichConstraint(i); 185 | return res; 186 | } 187 | 188 | /* ------------------------------------------------------------------------ */ 189 | /* --- DISPLAY ------------------------------------------------------------ */ 190 | /* ------------------------------------------------------------------------ */ 191 | 192 | void ActiveSet::disp(std::ostream& os, bool classic) const { 193 | if (classic) { 194 | os << " [ "; 195 | for (unsigned int r = 0; r < size(); ++r) { 196 | if (freerow[r]) continue; 197 | const unsigned int cst = mapInv(r); 198 | if (whichBound(cst) == Bound::BOUND_INF) 199 | os << "-"; 200 | else if (whichBound(cst) == Bound::BOUND_SUP) 201 | os << "+"; 202 | os << r << " "; 203 | } 204 | os << " ]"; 205 | } else { 206 | for (unsigned int i = 0; i < size(); ++i) { 207 | os << i << ": "; 208 | if (isActive(i)) 209 | os << cstMap[i].type; 210 | else 211 | os << "Unactive"; 212 | os << std::endl; 213 | } 214 | for (unsigned int i = 0; i < freerow.size(); ++i) { 215 | os << (freerow[i] ? "0" : "1"); 216 | } 217 | os << std::endl; 218 | } 219 | } 220 | std::ostream& operator<<(std::ostream& os, const ActiveSet& as) { 221 | as.disp(os); 222 | return os; 223 | } 224 | 225 | /* ------------------------------------------------------------------------ */ 226 | /* --- DEPRECATED --------------------------------------------------------- */ 227 | /* ------------------------------------------------------------------------ */ 228 | 229 | /* --- DEPRECATED --- */ 230 | /* DPC */ void ActiveSet:: 231 | /* DPC */ permuteRows(const VectorXi& P) 232 | /* DPC */ { 233 | /* DPC */ assert(false && "DEPRECATED"); 234 | /* DPC */ assert(P.size() == int(nba)); 235 | /* DPC */ VectorXi Pt(P.size()); 236 | /* DPC */ for (unsigned int i = 0; i < nba; ++i) 237 | Pt(P(i)) = i; 238 | /* DPC */ for (unsigned int i = 0; i < size(); ++i) 239 | /* DPC */ if (isActive(i)) 240 | cstMap[i].row = Pt(cstMap[i].row); 241 | /* DPC */} 242 | 243 | } // namespace soth 244 | -------------------------------------------------------------------------------- /exe/ijrr/ihqp.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * IJRR 2013 tests: compare the cost of the HQP with the QP cascade of Kanoun 4 | * and DeLassa. 5 | * 6 | * -------------------------------------------------------------------------- */ 7 | #define SOTH_DEBUG 8 | #define SOTH_DEBUG_MODE 45 9 | #include "COD.hpp" 10 | #include "RandomGenerator.hpp" 11 | #include "soth/HCOD.hpp" 12 | #include "soth/Random.hpp" 13 | #include "soth/debug.hpp" 14 | 15 | #include // for 'operator+=()' 16 | using namespace boost::assign; // bring 'operator+=()' into scope 17 | 18 | #ifndef WIN32 19 | #include 20 | #endif // WIN32 21 | 22 | #include 23 | #include 24 | #include "gettimeofday.hpp" 25 | 26 | using namespace soth; 27 | using std::cerr; 28 | using std::cout; 29 | using std::endl; 30 | 31 | /* -------------------------------------------------------------------------- */ 32 | /* -------------------------------------------------------------------------- */ 33 | /* -------------------------------------------------------------------------- */ 34 | 35 | struct NotificationToCout { 36 | void operator()(std::string stage, soth::ConstraintRef cst, 37 | std::string event) { 38 | sotDEBUG(0) << "At " << stage << ", " << cst << ": " << event << std::endl; 39 | count++; 40 | } 41 | static int count; 42 | NotificationToCout() { count = 0; } 43 | }; 44 | int NotificationToCout::count = 0; 45 | 46 | /* -------------------------------------------------------------------------- */ 47 | /* -------------------------------------------------------------------------- */ 48 | /* -------------------------------------------------------------------------- */ 49 | 50 | int cascadeqp(std::vector J, std::vector b, 51 | unsigned int NB_STAGE, unsigned int NC) { 52 | HCOD hsolver(NC, NB_STAGE); 53 | VectorXd solution(NC); 54 | NotificationToCout coutListen; 55 | std::vector Ir0; 56 | 57 | for (unsigned int i = 0; i < NB_STAGE; ++i) { 58 | sotDEBUG(15) << "Stage " << i << endl; 59 | hsolver.pushBackStage(J[i], b[i]); 60 | hsolver.notifiorRegistration(coutListen, i); 61 | hsolver.setNameByOrder("level"); 62 | /*hsolver.reset(); 63 | if(i==0) hsolver.setInitialActiveSet(); 64 | else 65 | { 66 | sotDEBUG(1) << "Init Ir0" << endl; 67 | Ir0.push_back(cstref_vector_t()); 68 | for( int ii=0;ii()->default_value(-1), 136 | "specify the seed of the random generator")( 137 | "size,S", po::value()->default_value(20), 138 | "size of the problem to try")("method,m", 139 | po::value()->default_value(0), 140 | "0 = HQP, 1 = Cascade, 2 = both")( 141 | "phase,p", po::value(), 142 | "Phase of the algo 1 = Decompo, 2 = inversion, 3 = projection"); 143 | 144 | po::positional_options_description p; 145 | p.add("seed", -1); 146 | p.add("method", 0); 147 | p.add("size", 20); 148 | 149 | po::store( 150 | po::command_line_parser(argc, argv).options(desc).positional(p).run(), 151 | optionMap); 152 | po::notify(optionMap); 153 | 154 | if (optionMap.count("help")) { 155 | std::cout << desc << std::endl; 156 | exit(0); 157 | } 158 | } 159 | 160 | /* Initialize the seed. */ 161 | { 162 | struct timeval tv; 163 | gettimeofday(&tv, NULL); 164 | int seed = tv.tv_usec % 7919; 165 | if (optionMap.count("seed")) { 166 | seed = optionMap["seed"].as(); 167 | } 168 | std::cout << "seed = " << seed << std::endl; 169 | soth::Random::setSeed(seed); 170 | } 171 | 172 | /* Decide the size of the problem. */ 173 | NB_STAGE = 4; 174 | NC = 20; 175 | NR += 9, 5, 7, 7; 176 | RANKFREE += 6, 3, 3, 6; 177 | RANKLINKED += 2, 2, 2, 0; 178 | 179 | sotDEBUG(5) << "NB_STAGE=" << NB_STAGE << ", NC=" << NC << endl; 180 | for (unsigned int k = 0; k < NB_STAGE; k++) { 181 | sotDEBUG(20) << RANKFREE[k] << " " << RANKLINKED[k] << " " << NR[k] << endl; 182 | } 183 | 184 | /* Initialize J and b. */ 185 | generateDeficientDataSet(J, b, NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 186 | 187 | for (unsigned int i = 0; i < NB_STAGE; ++i) { 188 | #ifndef NDEBUG 189 | std::cout << "J" << i + 1 << " = " << (soth::MATLAB)J[i] << std::endl; 190 | std::cout << "b" << i + 1 << " = " << b[i] << ";" << std::endl; 191 | #endif 192 | } 193 | 194 | #ifndef NDEBUG 195 | // sotDebugTrace::openFile(); 196 | #endif 197 | 198 | hqp(J, b, NB_STAGE, NC); 199 | cascadeqp(J, b, NB_STAGE, NC); 200 | 201 | #ifndef NDEBUG 202 | exit(0); 203 | #endif 204 | 205 | /* --------------------------------------------------------------------- */ 206 | /* --------------------------------------------------------------------- */ 207 | /* --------------------------------------------------------------------- */ 208 | struct timeval t0, t1; 209 | double totalTime = 0; 210 | std::ostringstream iss; 211 | cout << optionMap["size"].as() << endl; 212 | cout << optionMap["method"].as() << endl; 213 | iss << "/tmp/ihqp_" << optionMap["size"].as() << "_" 214 | << optionMap["method"].as() << ".dat"; 215 | std::ofstream datfile(iss.str().c_str()); 216 | 217 | for (int shoot = 0; shoot < 1000; shoot++) { 218 | double seed = Random::rand(); // % 704819; 219 | soth::Random::setSeed((int)seed); 220 | std::cout << "s" << seed << ":" << std::flush; 221 | 222 | generateFixedSizeRandomProfile(optionMap["size"].as(), 1, 0.8, 1, 223 | NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 224 | // 1.5,0.8,0.5,NB_STAGE,RANKFREE,RANKLINKED,NR,NC); 225 | // generateRandomProfile(NB_STAGE,RANKFREE,RANKLINKED,NR,NC); 226 | generateDeficientDataSet(J, b, NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 227 | 228 | try { 229 | gettimeofday(&t0, NULL); 230 | 231 | switch (optionMap["method"].as()) { 232 | case 0: 233 | datfile << hqp(J, b, NB_STAGE, NC); 234 | break; 235 | case 1: 236 | datfile << cascadeqp(J, b, NB_STAGE, NC); 237 | break; 238 | case 2: 239 | cout << "\t" << NB_STAGE; 240 | cout << "\t\tHQP:\t" << hqp(J, b, NB_STAGE, NC); 241 | cout << "\t\tCasQP:\t" << cascadeqp(J, b, NB_STAGE, NC) << endl; 242 | break; 243 | } 244 | 245 | gettimeofday(&t1, NULL); 246 | double time = (t1.tv_sec - t0.tv_sec) + (t1.tv_usec - t0.tv_usec) / 1.0e6; 247 | totalTime += time; 248 | 249 | datfile << "\t" << NB_STAGE << "\t" << time << "\t" << seed << std::endl; 250 | } catch (...) { 251 | cout << "Unsolved problem" << endl; 252 | } 253 | } 254 | std::cout << totalTime << std::endl; 255 | } 256 | -------------------------------------------------------------------------------- /unitTesting/ttrack.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Compute the optimum of a time-varying problem, to test the active-set 4 | * initial guess. 5 | * 6 | * -------------------------------------------------------------------------- */ 7 | #define SOTH_DEBUG 8 | #define SOTH_DEBUG_MODE 45 9 | #include "soth/BaseY.hpp" 10 | #include "soth/BasicStage.hpp" 11 | #include "soth/HCOD.hpp" 12 | #include "soth/Random.hpp" 13 | #include "soth/debug.hpp" 14 | 15 | #ifndef WIN32 16 | #include 17 | #endif // WIN32 18 | 19 | #include 20 | #include "RandomGenerator.hpp" 21 | #include "gettimeofday.hpp" 22 | 23 | using namespace soth; 24 | using std::endl; 25 | 26 | /* -------------------------------------------------------------------------- */ 27 | /* -------------------------------------------------------------------------- */ 28 | namespace soth { 29 | struct Now { 30 | long int sec, usec; 31 | Now(void) { 32 | struct timeval tref; 33 | gettimeofday(&tref, NULL); 34 | sec = tref.tv_sec; 35 | usec = tref.tv_usec; 36 | } 37 | }; 38 | double operator-(const Now& t1, const Now& t0) { 39 | return (double)(t1.sec - t0.sec) * 1000.0 + 40 | (double)(t1.usec - t0.usec + 0.0) / 1000.0; 41 | } 42 | std::ostream& operator<<(std::ostream& os, const Now& now) { 43 | return os << now.sec << "' " << now.usec; 44 | } 45 | 46 | } // namespace soth 47 | 48 | /* -------------------------------------------------------------------------- */ 49 | /* -------------------------------------------------------------------------- */ 50 | /* -------------------------------------------------------------------------- */ 51 | 52 | void increaseProblem( 53 | const double& DELTA, const unsigned int& NB_STAGE, const unsigned int& NC, 54 | const std::vector& NR, 55 | const std::vector& RANKLINKED, 56 | const std::vector& RANKFREE, 57 | const std::vector& NRA, std::vector& J, 58 | std::vector& Xhifree, std::vector& Jfree, 59 | std::vector& Xhilinked, 60 | std::vector& Alinked, std::vector& b) { 61 | for (unsigned int s = 0; s < NB_STAGE; ++s) { 62 | /* NRA[s] is the size of all previous stages 0:s-1 stacked. */ 63 | assert((RANKFREE[s] > 0) || (RANKLINKED[s] > 0)); 64 | 65 | Eigen::MatrixXd delta_Xhifree(NR[s], RANKFREE[s]); 66 | Eigen::MatrixXd delta_Jfree(RANKFREE[s], NC); 67 | Eigen::MatrixXd delta_Xhilinked(NR[s], RANKLINKED[s]); 68 | Eigen::MatrixXd delta_Alinked(RANKLINKED[s], NRA[s]); 69 | 70 | J[s].setZero(); 71 | if (RANKFREE[s] > 0) { 72 | soth::MatrixRnd::randomize(delta_Xhifree); 73 | soth::MatrixRnd::randomize(delta_Jfree); 74 | Jfree[s] += DELTA * delta_Jfree; 75 | Xhifree[s] += DELTA * delta_Xhifree; 76 | J[s] = Xhifree[s] * Jfree[s]; 77 | } 78 | if (RANKLINKED[s] > 0) { 79 | soth::MatrixRnd::randomize(delta_Xhilinked); 80 | soth::MatrixRnd::randomize(delta_Alinked); 81 | Xhilinked[s] += DELTA * delta_Xhilinked; 82 | Alinked[s] += DELTA * delta_Alinked; 83 | for (unsigned int sb = 0; sb < s; ++sb) { 84 | J[s] += Xhilinked[s] * 85 | Alinked[s].block(0, NRA[sb], RANKLINKED[s], NR[sb]) * J[sb]; 86 | } 87 | } 88 | 89 | for (unsigned int i = 0; i < NR[s]; ++i) { 90 | double x = 10 * DELTA * (Random::rand() * 2 - 1); 91 | double y = 10 * DELTA * (Random::rand() * 2 - 1); 92 | Bound::bound_t btype = b[s][i].getType(); 93 | switch (btype) { 94 | case Bound::BOUND_INF: 95 | case Bound::BOUND_SUP: 96 | case Bound::BOUND_TWIN: 97 | b[s][i] = Bound(b[s][i].getBound(btype) + x, btype); 98 | break; 99 | case Bound::BOUND_DOUBLE: 100 | b[s][i] = Bound(b[s][i].getBound(Bound::BOUND_INF) + x, 101 | b[s][i].getBound(Bound::BOUND_SUP) + y); 102 | break; 103 | case Bound::BOUND_NONE: 104 | assert(false && "Could never happen."); 105 | break; 106 | } 107 | } 108 | } 109 | } 110 | /* -------------------------------------------------------------------------- */ 111 | /* -------------------------------------------------------------------------- */ 112 | /* -------------------------------------------------------------------------- */ 113 | 114 | struct NotificationToCout { 115 | static int time; 116 | static bool touched; 117 | void operator()(std::string stage, soth::ConstraintRef cst, 118 | std::string event) { 119 | touched = true; 120 | std::cout << "At " << stage << ", " << cst << ", time " << time << ": " 121 | << event << std::endl; 122 | } 123 | void inc() { 124 | if (touched) { 125 | touched = false; 126 | std::cout << "--" << std::endl; 127 | } 128 | time++; 129 | } 130 | }; 131 | int NotificationToCout::time = 0; 132 | bool NotificationToCout::touched = 0; 133 | 134 | /* -------------------------------------------------------------------------- */ 135 | /* -------------------------------------------------------------------------- */ 136 | /* -------------------------------------------------------------------------- */ 137 | int main(int argc, char** argv) { 138 | #ifndef NDEBUG 139 | sotDebugTrace::openFile(); 140 | #endif 141 | 142 | unsigned int NB_STAGE, NC; 143 | std::vector NR, RANKLINKED, RANKFREE, NRA; 144 | std::vector J, Xhifree, Jfree, Xhilinked, Alinked; 145 | std::vector b; 146 | 147 | /* Initialize the seed. */ 148 | struct timeval tv; 149 | gettimeofday(&tv, NULL); 150 | int seed = (int)(tv.tv_usec % 7919); //= 7594; 151 | if (argc == 2) { 152 | seed = atoi(argv[1]); 153 | } 154 | std::cout << "seed = " << seed << std::endl; 155 | soth::Random::setSeed(seed); 156 | 157 | /* Decide the size of the problem. */ 158 | generateRandomProfile(NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 159 | 160 | /* Initialize J and b. */ 161 | J.resize(NB_STAGE); 162 | Xhifree.resize(NB_STAGE); 163 | Jfree.resize(NB_STAGE); 164 | Xhilinked.resize(NB_STAGE); 165 | Alinked.resize(NB_STAGE); 166 | NRA.resize(NB_STAGE); 167 | b.resize(NB_STAGE); 168 | 169 | /* Bulid the initial values. */ 170 | for (unsigned int s = 0; s < NB_STAGE; ++s) { 171 | /* NRA[s] is the size of all previous stages 0:s-1 stacked. */ 172 | if (s == 0) 173 | NRA[s] = 0; 174 | else 175 | NRA[s] = NRA[s - 1] + NR[s - 1]; 176 | b[s].resize(NR[s]); 177 | assert((RANKFREE[s] > 0) || (RANKLINKED[s] > 0)); 178 | 179 | J[s].resize(NR[s], NC); 180 | Xhifree[s].resize(NR[s], RANKFREE[s]); 181 | Jfree[s].resize(RANKFREE[s], NC); 182 | Xhilinked[s].resize(NR[s], RANKLINKED[s]); 183 | Alinked[s].resize(RANKLINKED[s], NRA[s]); 184 | 185 | J[s].setZero(); 186 | if (RANKFREE[s] > 0) { 187 | soth::MatrixRnd::randomize(Xhifree[s]); 188 | soth::MatrixRnd::randomize(Jfree[s]); 189 | J[s] += Xhifree[s] * Jfree[s]; 190 | } 191 | if (RANKLINKED[s] > 0) { 192 | soth::MatrixRnd::randomize(Xhilinked[s]); 193 | soth::MatrixRnd::randomize(Alinked[s]); 194 | for (unsigned int sb = 0; sb < s; ++sb) { 195 | J[s] += Xhilinked[s] * 196 | Alinked[s].block(0, NRA[sb], RANKLINKED[s], NR[sb]) * J[sb]; 197 | } 198 | } 199 | 200 | for (unsigned int i = 0; i < NR[s]; ++i) { 201 | double x = Random::rand() * 2; // DEBUG: should b U*2-1 202 | double y = Random::rand() * -2; // DEBUG 203 | int btype = randu(1, 4); 204 | switch (btype) { 205 | case 1: // = 206 | b[s][i] = x; 207 | break; 208 | case 2: // < 209 | b[s][i] = soth::Bound(y, soth::Bound::BOUND_INF); 210 | break; 211 | case 3: // > 212 | b[s][i] = soth::Bound(x, soth::Bound::BOUND_SUP); 213 | break; 214 | case 4: // < < 215 | b[s][i] = std::make_pair(std::min(x, y), std::max(x, y)); 216 | break; 217 | } 218 | } 219 | } 220 | 221 | std::cout << "NB_STAGE=" << NB_STAGE << ", NC=" << NC << endl; 222 | for (unsigned int i = 0; i < NB_STAGE; ++i) { 223 | sotDEBUG(1) << "J" << i + 1 << " = " << (soth::MATLAB)J[i] << std::endl; 224 | sotDEBUG(1) << "e" << i + 1 << " = " << b[i] << ";" << std::endl; 225 | } 226 | 227 | /* SOTH structure construction. */ 228 | soth::HCOD hcod(NC, NB_STAGE); 229 | for (unsigned int i = 0; i < NB_STAGE; ++i) { 230 | hcod.pushBackStage(NR[i], J[i].data()); 231 | } 232 | hcod.setNameByOrder("stage_"); 233 | NotificationToCout coutListen; 234 | hcod.notifiorRegistration(coutListen); 235 | 236 | /* Initial solve */ 237 | VectorXd solution; 238 | for (unsigned int i = 0; i < NB_STAGE; ++i) { 239 | hcod[i].getBoundsInternal() = b[i]; 240 | hcod[i].setInitialActiveSet(); 241 | sotDEBUG(1) << "J" << i + 1 << " = " << (soth::MATLAB)hcod[i].getJ() 242 | << std::endl; 243 | sotDEBUG(1) << "b" << i + 1 << " = " << hcod[i].getBounds() << std::endl; 244 | } 245 | hcod.activeSearch(solution); 246 | 247 | /* --- Tracking --- */ 248 | for (unsigned int t = 0; t < 100000; t++) { 249 | sotDEBUG(1) << " --- TIME t=" << t << " -----------------------------" 250 | << std::endl; 251 | coutListen.inc(); 252 | increaseProblem(1e-3, NB_STAGE, NC, NR, RANKLINKED, RANKFREE, NRA, J, 253 | Xhifree, Jfree, Xhilinked, Alinked, b); 254 | for (unsigned int i = 0; i < NB_STAGE; ++i) 255 | hcod[i].getBoundsInternal() = b[i]; 256 | hcod.activeSearch(solution); 257 | } 258 | } 259 | -------------------------------------------------------------------------------- /src/Stage.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __SOTH_STAGE__ 2 | #define __SOTH_STAGE__ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "soth/ActiveSet.hpp" 9 | #include "soth/Algebra.hpp" 10 | #include "soth/Allocator.hpp" 11 | #include "soth/BasicStage.hpp" 12 | #include "soth/Bound.hpp" 13 | #include "soth/Givens.hpp" 14 | #include "soth/SubMatrix.hpp" 15 | #include "soth/api.hpp" 16 | #include "soth/solvers.hpp" 17 | 18 | namespace soth { 19 | class BaseY; 20 | 21 | /* --- STAGE -------------------------------------------------------------- */ 22 | /* --- STAGE -------------------------------------------------------------- */ 23 | /* --- STAGE -------------------------------------------------------------- */ 24 | 25 | class SOTH_EXPORT Stage : public BasicStage { 26 | public: 27 | typedef MatrixXd::Index Index; 28 | typedef SubMatrix::RowIndices Indirect; 29 | 30 | protected: 31 | using BasicStage::bounds; 32 | using BasicStage::J; 33 | using BasicStage::nc; 34 | using BasicStage::nr; 35 | using BasicStage::Y; 36 | 37 | /* Work range matrices. */ 38 | MatrixXd W_; 39 | MatrixXd ML_; 40 | VectorXd e_; 41 | VectorXd lambda_; 42 | 43 | /* fullRankRows = Ir. defRankRows = In. 44 | * Ir = L.indirectRows() -- Irn = M.indirectRows(). In =D= Irn/Ir. */ 45 | Indirect Ir, Irn, Iw, Im, Il; 46 | 47 | /* W = W_( :,[In Ir] ). 48 | * M = ML_( [In Ir],0:sizeM-1 ). 49 | * L = ML_( Ir,sizeM:sizeM+sizeL-1 ). 50 | * Lo = [0;L] = ML_( [In Ir],sizeM:sizeM+sizeL-1 ). 51 | * 52 | * W_*ML_ = W*[M [ zeros(In.size(),rank); L ] ] 53 | */ 54 | SubMatrixXd M, L; 55 | SubMatrixXd W; 56 | SubMatrixXd Wr, Mr; 57 | SubVectorXd e, lambda; 58 | mutable VectorXd lambdadamped; // For debug only, can be removed on release. 59 | VectorXd lzfreezed; 60 | 61 | bool isWIdenty; 62 | /* sizeL = card(Ir). sizeM = previousRank. */ 63 | Index sizeM, sizeL; 64 | 65 | /* Memory allocators. */ 66 | SubActiveSet, Indirect> activeSet; 67 | AllocatorML freeML; 68 | 69 | /* Damping */ 70 | MatrixXd Ld_, Ldwork_; 71 | mutable VectorXd edwork_; 72 | SubMatrixXd Ld, Ldwork; 73 | mutable SubVectorXd edwork; 74 | GivensSequence Wd; 75 | double dampingFactor; 76 | const static double DAMPING_FACTOR; 77 | 78 | /* Check if the stage has been reset, initialized, if the optimum 79 | * has been computed, and if the lagrange multipliers have been 80 | * computed. */ 81 | bool isReset, isInit, isOptimumCpt, isLagrangeCpt, isDampCpt, isFreezed; 82 | 83 | public: 84 | Stage(const MatrixXd& J, const VectorBound& bounds, BaseY& Y); 85 | Stage(const Index nr, const Index nc, const double* Jdata, const Bound* bdata, 86 | const BaseY& Y); 87 | Stage(const Index nr, const Index nc, const double* Jdata, const BaseY& Y); 88 | 89 | /* --- INIT ------------------------------------------------------------- */ 90 | void setInitialActiveSet(void); 91 | void setInitialActiveSet(const cstref_vector_t& initialGuess, 92 | bool checkTwin = false); 93 | cstref_vector_t getOptimalActiveSet(bool withTwin = false); 94 | 95 | void reset(void); 96 | /* Return the rank of the current COD = previousRank+size(L). 97 | * Give a non-const ref on Y so that it is possible to modify it. 98 | */ 99 | void computeInitialCOD(BaseY& Yinit); 100 | 101 | protected: 102 | void nullifyLineDeficient(const Index row, const Index in_r); 103 | void computeInitialJY(); 104 | void conditionalWinit(bool id); 105 | /* --- DOWN ------------------------------------------------------------- */ 106 | public: 107 | /* Return true if the rank re-increase operated at the current stage. */ 108 | bool downdate(const Index position, GivensSequence& Ydown); 109 | /* Return true if the rank decrease operated at the current stage. */ 110 | bool propagateDowndate(GivensSequence& Ydown, bool decreasePreviousRank); 111 | 112 | protected: 113 | void regularizeHessenberg(GivensSequence& Ydown); 114 | Index nullifyACrossFromW(const Index position); 115 | void removeARowFromL(Index row); 116 | void removeACrossFromW(const Index& row, const Index& col); 117 | 118 | /* --- UPD -------------------------------------------------------------- */ 119 | public: 120 | /* Return the rank of the line where the rank re-decrease will occurs. */ 121 | Index update(const ConstraintRef& cst, GivensSequence& Yup); 122 | void propagateUpdate(GivensSequence& Ydown, Index decreasePreviousRank); 123 | 124 | protected: 125 | void addARow(const Index& mlrowup, bool deficient = false); 126 | 127 | /* --- SOLVE ------------------------------------------------------------ */ 128 | public: 129 | /* Solve in the Y space. The solution has then to be multiply by Y: u = Y*Yu. 130 | */ 131 | void computeSolution(VectorXd& Ytu) const; 132 | 133 | void damp(void); 134 | template 135 | void applyDamping(MatrixBase& x) const; 136 | template 137 | void applyDamping(MatrixBase& x, MatrixBase& y) const; 138 | template 139 | void applyDampingTranspose(MatrixBase& x) const; 140 | template 141 | void applyDampingTranspose(MatrixBase& x, 142 | const MatrixBase& y) const; 143 | 144 | void damping(const double& factor) { dampingFactor = factor; } 145 | double damping(void) const { return dampingFactor; } 146 | bool useDamp(void) const { return isDampCpt; } 147 | void dampBoundValue(const ConstraintRef& cst, const double& value); 148 | 149 | /* --- MULTIPLICATORS --------------------------------------------------- */ 150 | public: 151 | /* The const functions simultaneously set up the lambda member. */ 152 | template 153 | void computeError(const VectorXd& Ytu, MatrixBase& err) const; 154 | void computeError(const VectorXd& Ytu); 155 | /* computeRho(.,.,false) is const. What trick can we use to explicit that? 156 | * TODO. */ 157 | void computeRho(const VectorXd& Ytu, VectorXd& Ytrho, bool inLambda = false); 158 | template 159 | void computeLagrangeMultipliers(VectorXd& rho, MatrixBase& l) const; 160 | void computeLagrangeMultipliers(VectorXd& rho); 161 | 162 | protected: 163 | void computeErrorFromJu(const VectorXd& MLYtu); 164 | template 165 | void computeErrorFromJu(const VectorXd& Ytu, MatrixBase& err) const; 166 | void computeMLYtu(const VectorXd& Ytu, VectorXd& MLYtu) const; 167 | 168 | /* --- ACTIVE SEARCH ---------------------------------------------------- */ 169 | public: 170 | /* Return true if all bounds are checked with the specified tau. If tau is 171 | * specified, the step is computed btw (with tau_out <= tau_in) and the 172 | * constraint to update is returned. 173 | */ 174 | bool checkBound(const VectorXd& u0, const VectorXd& u1, ConstraintRef*, 175 | double* tau); 176 | bool checkBound(const VectorXd& u0, const VectorXd& u1, ConstraintRef& cstmax, 177 | double& taumax); 178 | bool maxLambda(const VectorXd& u, double& lmax, Index& row) const; 179 | void freezeSlacks(const bool& slacks = true); 180 | 181 | /* --- CHECK ------------------------------------------------------------ */ 182 | public: 183 | /* WMLY = [ W*M W(:,1:rank)*L zeros(sizeA,nc-sizeM-sizeL) ]*Y' */ 184 | void recompose(MatrixXd& WMLY) const; 185 | void show(std::ostream& os, Index stageRef, bool check = false) const; 186 | void showActiveSet(std::ostream& os) const; 187 | 188 | /* Return a sub matrix containing the active rows of J, in the 189 | * same order as given by W. J_ is a matrix where th full 190 | * J is stored (workspace). */ 191 | SubMatrix Jactive(MatrixXd& J_) const; 192 | MatrixXd Jactive() const; 193 | /* Return a sub vector containing the active rows of e, in the 194 | * same order as given by W. */ 195 | SubVectorXd eactive(VectorXd& e_) const; 196 | VectorXd eactive() const; 197 | 198 | bool testRecomposition(void) const; 199 | bool testSolution(const VectorXd& solution) const; 200 | bool testUnactiveTwins(void); 201 | 202 | /* For debug purpose, give the line of an active constraint (assert the 203 | * activity). */ 204 | Index where(Index cst) const; 205 | ConstraintRef which(Index row) const; 206 | bool isActive(Index cst) const; 207 | 208 | public: 209 | /* --- ACCESSORS --- */ 210 | typedef TriangularView TriSubMatrixXd; 211 | typedef TriangularView const_TriSubMatrixXd; 212 | typedef VectorBlock RowL; 213 | typedef MatrixXd::RowXpr RowML; 214 | 215 | SubMatrixXd getM() { return M; } 216 | const_SubMatrixXd getM() const { return M; } 217 | SubMatrixXd getL() { return L; } 218 | const_SubMatrixXd getL() const { return L; } 219 | TriSubMatrixXd getLtri() { return L.triangularView(); } 220 | const_TriSubMatrixXd getLtri() const { return L.triangularView(); } 221 | TriSubMatrixXd getLdtri() { return Ld.triangularView(); } 222 | const_TriSubMatrixXd getLdtri() const { return Ld.triangularView(); } 223 | SubVectorXd gete() { return e; } 224 | const_SubVectorXd gete() const { return e; } 225 | SubVectorXd getLagrangeMultipliers() { return lambda; } 226 | const_SubVectorXd getLagrangeMultipliers() const { return lambda; } 227 | MatrixXd getWr() const { 228 | if (isWIdenty) 229 | return MatrixXd::Identity(sizeL, sizeL); 230 | else 231 | return Wr; 232 | } 233 | 234 | VectorXd getLagrangeDamped() const { return lambdadamped; } 235 | 236 | RowL rowL0(const Index r); 237 | RowML rowMrL0(const Index r); 238 | RowL rowML(const Index r); 239 | Index rowSize(const Index r); 240 | 241 | /* TODO: sizeL and sizeM should be automatically determined from the 242 | * corresponding indexes. */ 243 | inline Index nbConstraints(void) const { return nr; } 244 | inline Index sizeA(void) const { return activeSet.nbActive(); } 245 | // sizeN = card(In) = sizeA-sizeL. 246 | inline Index sizeN(void) const { 247 | assert((int)(sizeA() - sizeL) >= 0); 248 | return sizeA() - sizeL; 249 | } 250 | inline Index rank() const { return sizeL; } 251 | 252 | inline Index getSizeM() const { return sizeM; } 253 | inline Index getSizeL() const { return sizeL; } 254 | 255 | using BasicStage::getBoundRow; 256 | using BasicStage::getJrow; 257 | 258 | public: 259 | static double EPSILON; 260 | 261 | public: /* For debug purpose, could be remove on RELEASE. */ 262 | std::string name; 263 | }; 264 | 265 | } // namespace soth 266 | 267 | #endif // #ifndef __SOTH_STAGE__ 268 | -------------------------------------------------------------------------------- /src/debug.hpp: -------------------------------------------------------------------------------- 1 | /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 2 | * Copyright Projet Lagadic, 2005 3 | *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 4 | * 5 | * File: sotDebug.h 6 | * Project: STack of Tasks 7 | * Author: Nicolas Mansard 8 | * 9 | * Version control 10 | * =============== 11 | * 12 | * $Id$ 13 | * 14 | * Description 15 | * ============ 16 | * 17 | * Macro de trace et de debugage 18 | * 19 | * - TRACAGE: TRACE et ERROR_TRACE fonctionnent comme des printf 20 | * avec retour chariot en fin de fonction. 21 | * CERROR et CTRACE fonctionnent comme les flux de sortie 22 | * C++ cout et cerr. 23 | * - DEBUGAGE: DEBUG_TRACE(niv, et DERROR_TRACE(niv, fonctionnent 24 | * comme des printf, n'imprimant que si le niveau de trace 'niv' est 25 | * superieur au mode de debugage SOTH_DEBUG_MODE. 26 | * CDEBUG(niv) fonctionne comme le flux de sortie C++ cout. 27 | * DEBUG_ENABLE(niv) vaut 1 ssi le niveau de tracage 'niv' 28 | * est superieur au mode de debugage DEBUG_MODE. Il vaut 0 sinon. 29 | * - PROG DEFENSIVE: DEFENSIF(a) vaut a ssi le mode defensif est active, 30 | * et vaut 0 sinon. 31 | * 32 | * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 33 | 34 | #ifndef __SOTH_DEBUG_HH 35 | #define __SOTH_DEBUG_HH 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include "soth/api.hpp" 43 | 44 | /* -------------------------------------------------------------------------- */ 45 | /* -------------------------------------------------------------------------- */ 46 | /* -------------------------------------------------------------------------- */ 47 | #ifdef NDEBUG 48 | //# undef SOTH_DEBUG 49 | #endif 50 | 51 | namespace soth { 52 | #ifndef SOTH_DEBUG_MODE 53 | #define SOTH_DEBUG_MODE 0 54 | #endif 55 | #ifndef SOTH_TEMPLATE_DEBUG_MODE 56 | #define SOTH_TEMPLATE_DEBUG_MODE 0 57 | #endif 58 | 59 | #define SOT_COMMON_TRACES \ 60 | do { \ 61 | va_list arg; \ 62 | va_start(arg, format); \ 63 | vsnprintf(charbuffer, SIZE, format, arg); \ 64 | va_end(arg); \ 65 | outputbuffer << tmpbuffer.str() << charbuffer << std::endl; \ 66 | } while (0) 67 | 68 | class SOTH_EXPORT sotDebugTrace { 69 | public: 70 | static const int SIZE = 512; 71 | 72 | std::stringstream tmpbuffer; 73 | std::ostream& outputbuffer; 74 | std::ofstream voidbuffer; 75 | char charbuffer[SIZE + 1]; 76 | int traceLevel; 77 | int traceLevelTemplate; 78 | int debugPrior; 79 | 80 | sotDebugTrace(std::ostream& os) : outputbuffer(os), debugPrior(0) {} 81 | 82 | inline void trace(const int level, const char* format, ...) { 83 | if (level + debugPrior <= traceLevel) SOT_COMMON_TRACES; 84 | tmpbuffer.str(""); 85 | } 86 | inline void trace(const char* format, ...) { 87 | SOT_COMMON_TRACES; 88 | tmpbuffer.str(""); 89 | } 90 | inline void trace(const int level = -1) { 91 | if (level + debugPrior <= traceLevel) outputbuffer << tmpbuffer.str(); 92 | tmpbuffer.str(""); 93 | } 94 | 95 | inline void traceTemplate(const int level, const char* format, ...) { 96 | if (level + debugPrior <= traceLevelTemplate) SOT_COMMON_TRACES; 97 | tmpbuffer.str(""); 98 | } 99 | inline void traceTemplate(const char* format, ...) { 100 | SOT_COMMON_TRACES; 101 | tmpbuffer.str(""); 102 | } 103 | 104 | inline sotDebugTrace& pre(const std::ostream&) { return *this; } 105 | inline sotDebugTrace& pre(const std::ostream&, int level) { 106 | traceLevel = level; 107 | return *this; 108 | } 109 | /* inline sotDebugTrace& preTemplate( const std::ostream& dummy,int level 110 | * ) */ 111 | /* { traceLevelTemplate = level; return *this; } */ 112 | 113 | static const char* DEBUG_FILENAME_DEFAULT; 114 | static void openFile(const char* filename = DEBUG_FILENAME_DEFAULT); 115 | static void closeFile(const char* filename = DEBUG_FILENAME_DEFAULT); 116 | 117 | operator std::ostream&() { return outputbuffer; } 118 | }; 119 | 120 | SOTH_EXPORT extern sotDebugTrace sotDEBUGFLOW; 121 | SOTH_EXPORT extern sotDebugTrace sotERRORFLOW; 122 | 123 | #ifdef SOTH_DEBUG 124 | #if 0 125 | #define sotPREDEBUG \ 126 | "% " << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") :\n" 127 | #define sotPREERROR \ 128 | "\t!! " << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") :" 129 | #else 130 | #define sotPREDEBUG \ 131 | "% " \ 132 | << ": " << __FUNCTION__ << "(#" << __LINE__ << ") :\n" 133 | #define sotPREERROR \ 134 | "\t!! " \ 135 | << ": " << __FUNCTION__ << "(#" << __LINE__ << ") :" 136 | #endif 137 | #define sotDEBUG(level) \ 138 | if ((level + soth::sotDEBUGFLOW.debugPrior > SOTH_DEBUG_MODE) || \ 139 | (!soth::sotDEBUGFLOW.outputbuffer.good())) \ 140 | ; \ 141 | else \ 142 | soth::sotDEBUGFLOW.outputbuffer << sotPREDEBUG 143 | #define sotDEBUGMUTE(level) \ 144 | if ((level + soth::sotDEBUGFLOW.debugPrior > SOTH_DEBUG_MODE) || \ 145 | (!soth::sotDEBUGFLOW.outputbuffer.good())) \ 146 | ; \ 147 | else \ 148 | soth::sotDEBUGFLOW.outputbuffer 149 | #define sotERROR \ 150 | if (!soth::sotDEBUGFLOW.outputbuffer.good()) \ 151 | ; \ 152 | else \ 153 | sotERRORFLOW.outputbuffer << sotPREERROR 154 | #define sotDEBUGF \ 155 | if (!soth::sotDEBUGFLOW.outputbuffer.good()) \ 156 | ; \ 157 | else \ 158 | soth::sotDEBUGFLOW \ 159 | .pre(soth::sotDEBUGFLOW.tmpbuffer << sotPREDEBUG, SOTH_DEBUG_MODE) \ 160 | .trace 161 | #define sotERRORF \ 162 | if (!soth::sotDEBUGFLOW.outputbuffer.good()) \ 163 | ; \ 164 | else \ 165 | sotERRORFLOW.pre(sotERRORFLOW.tmpbuffer << sotPREERROR).trace 166 | // TEMPLATE 167 | #define sotTDEBUG(level) \ 168 | if ((level + soth::sotDEBUGFLOW.debugPrior > SOTH_TEMPLATE_DEBUG_MODE) || \ 169 | (!soth::sotDEBUGFLOW.outputbuffer.good())) \ 170 | ; \ 171 | else \ 172 | soth::sotDEBUGFLOW.outputbuffer << sotPREDEBUG 173 | #define sotTDEBUGF \ 174 | if (!soth::sotDEBUGFLOW.outputbuffer.good()) \ 175 | ; \ 176 | else \ 177 | soth::sotDEBUGFLOW \ 178 | .pre(soth::sotDEBUGFLOW.tmpbuffer << sotPREDEBUG, \ 179 | SOTH_TEMPLATE_DEBUG_MODE) \ 180 | .trace 181 | inline bool sotDEBUG_ENABLE(const int& level) { 182 | return level <= SOTH_DEBUG_MODE; 183 | } 184 | inline bool sotTDEBUG_ENABLE(const int& level) { 185 | return level <= SOTH_TEMPLATE_DEBUG_MODE; 186 | } 187 | 188 | class sotDEBUGPRIORclass { 189 | protected: 190 | int previousLevel; 191 | sotDEBUGPRIORclass(void){}; 192 | 193 | public: 194 | sotDEBUGPRIORclass(int prior) : previousLevel(sotDEBUGFLOW.debugPrior) { 195 | sotDEBUGFLOW.debugPrior += prior; 196 | } 197 | ~sotDEBUGPRIORclass(void) { sotDEBUGFLOW.debugPrior = previousLevel; } 198 | }; 199 | 200 | #define sotDEBUGPRIOR(a) sotDEBUGPRIORclass sotDEBUGPRIORclass##__FUNCTION__(a); 201 | 202 | /* -------------------------------------------------------------------------- */ 203 | #else // #ifdef SOTH_DEBUG 204 | #if 0 205 | #define sotPREERROR \ 206 | "\t!! " << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") :" 207 | #else 208 | #define sotPREERROR \ 209 | "\t!! " \ 210 | << ": " << __FUNCTION__ << "(#" << __LINE__ << ") :" 211 | 212 | #endif 213 | #define sotDEBUG(level) \ 214 | if (1) \ 215 | ; \ 216 | else \ 217 | std::cout 218 | #define sotDEBUGMUTE(level) \ 219 | if (1) \ 220 | ; \ 221 | else \ 222 | std::cout 223 | #define sotERROR sotERRORFLOW.outputbuffer << sotPREERROR 224 | #define sotDEBUGPRIOR(a) \ 225 | do { \ 226 | } while (0) 227 | inline void sotDEBUGF(const int, const char*, ...) { return; } 228 | inline void sotDEBUGF(const char*, ...) { return; } 229 | inline void sotERRORF(const int, const char*, ...) { return; } 230 | inline void sotERRORF(const char*, ...) { return; } 231 | // TEMPLATE 232 | #define sotTDEBUG(level) \ 233 | if (1) \ 234 | ; \ 235 | else \ 236 | std::cout 237 | inline void sotTDEBUGF(const int, const char*, ...) { return; } 238 | inline void sotTDEBUGF(const char*, ...) { return; } 239 | #define sotDEBUG_ENABLE(level) false 240 | #define sotTDEBUG_ENABLE(level) false 241 | 242 | #endif // #ifdef SOTH_DEBUG 243 | /* -------------------------------------------------------------------------- */ 244 | 245 | #define sotDEBUGIN(level) sotDEBUG(level) << "# In {" << std::endl 246 | #define sotDEBUGOUT(level) sotDEBUG(level) << "# Out }" << std::endl 247 | #define sotDEBUGINOUT(level) sotDEBUG(level) << "# In/Out { }" << std::endl 248 | 249 | #define sotTDEBUGIN(level) sotTDEBUG(level) << "# In {" << std::endl 250 | #define sotTDEBUGOUT(level) sotTDEBUG(level) << "# Out }" << std::endl 251 | #define sotTDEBUGINOUT(level) sotTDEBUG(level) << "# In/Out { }" << std::endl 252 | 253 | } // namespace soth 254 | 255 | /* Prevent unused warning */ 256 | #define UNUSED(x) ((void)x) 257 | 258 | #endif /* #ifdef __SOTH_DEBUG_HH */ 259 | 260 | /* 261 | * Local variables: 262 | * c-basic-offset: 4 263 | * End: 264 | */ 265 | -------------------------------------------------------------------------------- /exe/ijrr/ehqp.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Time comparisons for IJRR 2013 (sub) paper. 4 | * 5 | * -------------------------------------------------------------------------- */ 6 | #define SOTH_DEBUG 7 | #define SOTH_DEBUG_MODE 45 8 | #include "COD.hpp" 9 | #include "RandomGenerator.hpp" 10 | #include "soth/HCOD.hpp" 11 | #include "soth/Random.hpp" 12 | #include "soth/debug.hpp" 13 | 14 | #ifndef WIN32 15 | #include 16 | #endif // WIN32 17 | 18 | #include 19 | #include 20 | #include "gettimeofday.hpp" 21 | 22 | using namespace soth; 23 | using std::cerr; 24 | using std::cout; 25 | using std::endl; 26 | 27 | /* -------------------------------------------------------------------------- */ 28 | /* -------------------------------------------------------------------------- */ 29 | /* -------------------------------------------------------------------------- */ 30 | #include 31 | using Eigen::JacobiSVD; 32 | 33 | VectorXd svdSolve(JacobiSVD &svd, VectorXd &ref, 34 | double *rankPtr = NULL, double EPS = 1e-6) { 35 | typedef JacobiSVD SVDType; 36 | const JacobiSVD::MatrixUType &U = svd.matrixU(); 37 | const JacobiSVD::MatrixVType &V = svd.matrixV(); 38 | const JacobiSVD::SingularValuesType &s = svd.singularValues(); 39 | // std::cout << (MATLAB)U<< (MATLAB)s<<(MATLAB)V << std::endl; 40 | 41 | const int N = V.rows(), M = U.rows(), S = std::min(M, N); 42 | 43 | int rank = 0; 44 | while (rank < S && s[rank] > EPS) rank++; 45 | 46 | VectorXd sinv = s.head(rank).array().inverse(); 47 | 48 | // std::cout << "U = " << (MATLAB)U.leftCols(rank) << std::endl; 49 | // std::cout << "V = " << (MATLAB)V.leftCols(rank) << std::endl; 50 | // std::cout << "S = " << (MATLAB)(MatrixXd)sinv.asDiagonal() << std::endl; 51 | 52 | VectorXd x; 53 | x = V.leftCols(rank) * sinv.asDiagonal() * U.leftCols(rank).transpose() * ref; 54 | 55 | if (rankPtr != NULL) *rankPtr = rank; 56 | 57 | return x; 58 | } 59 | 60 | int stageIter = 0; 61 | int phase = 3; 62 | int ratioSvd = 1; 63 | 64 | void siciliano(const std::vector &A, 65 | std::vector &b, const unsigned int NB_STAGE, 66 | const std::vector &RANKFREE, 67 | const std::vector & /*RANKLINKED*/, 68 | const std::vector & /*NR*/, 69 | const unsigned int NC) { 70 | VectorXd x(NC); 71 | x.setZero(); 72 | MatrixXd P(NC, NC); 73 | P.setIdentity(); 74 | int rank = NC; 75 | COD Akcod(ComputeFullU | ComputeThinV); 76 | MatrixXd AkP(NC * 2, NC); 77 | 78 | for (unsigned int k = 0; k < NB_STAGE; ++k) { 79 | for (int i = 0; i < ratioSvd; ++i) Akcod.compute(A[k] * P); 80 | 81 | if (phase > 1) { 82 | VectorXd blim = b[k] - A[k] * x; 83 | x += Akcod.solve(blim); 84 | } 85 | 86 | if (phase > 2) { 87 | P -= Akcod.matrixVr() * Akcod.matrixVr().transpose(); 88 | } 89 | 90 | rank = rank - RANKFREE[k]; 91 | } 92 | } 93 | 94 | void hsvd(const std::vector &A, std::vector &b, 95 | const unsigned int NB_STAGE, 96 | const std::vector &RANKFREE, 97 | const std::vector & /*RANKLINKED*/, 98 | const std::vector & /*NR*/, const unsigned int NC) { 99 | VectorXd x(NC); 100 | x.setZero(); 101 | MatrixXd Z(NC, NC); 102 | Z.setIdentity(); 103 | MatrixXd Ztmp(NC, NC); 104 | MatrixXd P(NC, NC); 105 | P.setIdentity(); 106 | int rank = NC; 107 | COD Akcod(ComputeFullU); 108 | for (unsigned int k = 0; k < NB_STAGE; ++k) { 109 | /* 110 | JacobiSVD Aksvd(A[k]*Z, ComputeThinU | ComputeThinV); 111 | VectorXd blim = b[k] - A[k]*x; 112 | x += Z*svdSolve(Aksvd,blim,&rank); 113 | Z = Aksvd.matrixV().rightCols(rank); 114 | */ 115 | 116 | for (int i = 0; i < ratioSvd; ++i) Akcod.compute(A[k] * Z.rightCols(rank)); 117 | 118 | if (phase > 1) { 119 | VectorXd blim = b[k] - A[k] * x; 120 | x += Z * Akcod.solve(blim); 121 | } 122 | 123 | if (phase > 2) { 124 | MatrixXd::ColsBlockXpr Zr = Z.rightCols(rank); 125 | Akcod.applyMatrixV(Zr); 126 | } 127 | 128 | rank = rank - RANKFREE[k]; 129 | } 130 | } 131 | 132 | void hcod(std::vector &A, std::vector &b, 133 | const unsigned int NB_STAGE, 134 | const std::vector &RANKFREE, 135 | const std::vector & /*RANKLINKED*/, 136 | const std::vector & /*NR*/, const unsigned int NC) { 137 | VectorXd x(NC); 138 | x.setZero(); 139 | VectorXd y(NC); 140 | y.setZero(); 141 | MatrixXd Z(NC, NC); 142 | Z.setIdentity(); 143 | MatrixXd Ztmp(NC, NC); 144 | MatrixXd P(NC, NC); 145 | P.setIdentity(); 146 | double rank = NC; 147 | std::vector Akcod(NB_STAGE); 148 | std::vector na(NB_STAGE + 1); 149 | na[0] = NC; 150 | 151 | for (unsigned int k = 0; k < NB_STAGE; ++k) { 152 | /* 153 | AkZ = Ak Z1 Z2 ... Zk-1 154 | y = L^+ b 155 | x = Z y = Z1 Z2 ... Zk-1 y 156 | */ 157 | 158 | Akcod[k].options(ComputeFullU); 159 | 160 | for (unsigned int i = 0; i < k; ++i) { 161 | // std::cout << "AkZ " << i << "," << k<< std::endl; 162 | MatrixXd::ColsBlockXpr AkZ = A[k].rightCols(na[i]); 163 | Akcod[i].applyMatrixV(AkZ); 164 | } 165 | // std::cout << "decomp" << na[k] << std::endl; 166 | Akcod[k].compute(A[k].rightCols(na[k])); 167 | 168 | if (phase > 1) { 169 | VectorXd blim = b[k] - A[k] * y; 170 | // std::cout << "solve y" << std::endl; 171 | y.segment(NC - na[k], RANKFREE[k]) = Akcod[k].solve(blim, true); 172 | } 173 | 174 | na[k + 1] = na[k] - RANKFREE[k]; 175 | rank = rank - RANKFREE[k]; 176 | } 177 | 178 | if (phase > 2) { 179 | // std::cout << "Phase 3 " << std::endl; 180 | for (int i = NB_STAGE - 1; i >= 0; --i) { 181 | // std::cout << "Zyk " << i << std::endl; 182 | y.tail(na[i]).applyOnTheLeft(Akcod[i].qrv.householderQ()); 183 | } 184 | } 185 | } 186 | 187 | /* -------------------------------------------------------------------------- */ 188 | /* -------------------------------------------------------------------------- */ 189 | /* -------------------------------------------------------------------------- */ 190 | 191 | #include 192 | #include 193 | 194 | int main(int argc, char **argv) { 195 | #ifndef NDEBUG 196 | sotDebugTrace::openFile(); 197 | #endif 198 | 199 | unsigned int NB_STAGE, NC; 200 | std::vector NR, RANKLINKED, RANKFREE; 201 | std::vector J; 202 | std::vector e; 203 | std::vector b; 204 | 205 | boost::program_options::variables_map optionMap; 206 | { 207 | namespace po = boost::program_options; 208 | po::options_description desc("trandom options"); 209 | desc.add_options()("help", "produce help message")( 210 | "seed,s", po::value(), "specify the seed of the random generator")( 211 | "size,S", po::value(), "size of the problem to try")( 212 | "method,m", po::value(), "0 = SICILIANO COD, 1 = COD Z, 2 = HCOD")( 213 | "denoise,n", po::value(), 214 | "Run n times the algo to remove the computation-time noise")( 215 | "ratio,r", po::value(), "ratio of the SVD wrt COD")( 216 | "phase,p", po::value(), 217 | "Phase of the algo 1 = Decompo, 2 = inversion, 3 = projection"); 218 | 219 | po::positional_options_description p; 220 | p.add("seed", -1); 221 | p.add("method", 0); 222 | p.add("size", 20); 223 | p.add("denoise", 1); 224 | 225 | po::store( 226 | po::command_line_parser(argc, argv).options(desc).positional(p).run(), 227 | optionMap); 228 | po::notify(optionMap); 229 | 230 | if (optionMap.count("help")) { 231 | std::cout << desc << std::endl; 232 | exit(0); 233 | } 234 | 235 | if (optionMap.count("ratio")) ratioSvd = optionMap["ratio"].as(); 236 | if (optionMap.count("phase")) phase = optionMap["phase"].as(); 237 | } 238 | 239 | /* Initialize the seed. */ 240 | struct timeval tv; 241 | gettimeofday(&tv, NULL); 242 | int seed = tv.tv_usec % 7919; 243 | if (optionMap.count("seed")) { 244 | seed = optionMap["seed"].as(); 245 | } 246 | std::cout << "seed = " << seed << std::endl; 247 | soth::Random::setSeed(seed); 248 | 249 | /* Decide the size of the problem. */ 250 | generateFixedSizeRandomProfile(optionMap["size"].as(), 1.2, 0.8, 0.5, 251 | NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 252 | std::cout << "NB_STAGE=" << NB_STAGE << ", NC=" << NC << endl; 253 | for (int k = 0; k < (int)NB_STAGE; k++) { 254 | sotDEBUG(20) << RANKFREE[k] << " " << RANKLINKED[k] << " " << NR[k] << endl; 255 | } 256 | 257 | /* Initialize J and b. */ 258 | generateDeficientDataSet(J, b, NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 259 | 260 | cout << endl; 261 | e.resize(NB_STAGE); 262 | for (unsigned int i = 0; i < NB_STAGE; ++i) { 263 | // std::cout << "e"<() << "_" 280 | << optionMap["method"].as() << "_" << optionMap["phase"].as() 281 | << "_" << optionMap["ratio"].as() << ".dat"; 282 | 283 | std::ofstream datfile(iss.str().c_str()); 284 | { 285 | std::ofstream ftrace("/tmp/sici.dat"); // reset 286 | } 287 | 288 | for (int shoot = 0; shoot < 1000; shoot++) { 289 | double seed = Random::rand(); // % 704819; 290 | soth::Random::setSeed((int)seed); 291 | std::cout << "s" << seed << ":" << std::flush; 292 | 293 | generateFixedSizeRandomProfile(optionMap["size"].as(), 1.2, 0.8, 0.5, 294 | NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 295 | generateDeficientDataSet(J, b, NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 296 | e.resize(NB_STAGE); 297 | for (unsigned int i = 0; i < NB_STAGE; ++i) { 298 | e[i].resize(NR[i]); 299 | soth::MatrixRnd::randomize(e[i]); 300 | for (unsigned int r = 0; r < NR[i]; ++r) b[i][r] = e[i][r]; 301 | } 302 | 303 | HCOD hsolver(NC, NB_STAGE); 304 | for (unsigned int i = 0; i < NB_STAGE; ++i) 305 | hsolver.pushBackStage(J[i], b[i]); 306 | 307 | hsolver.reset(); 308 | hsolver.setInitialActiveSet(); 309 | 310 | gettimeofday(&t0, NULL); 311 | for (int noise = 0; noise < optionMap["denoise"].as(); ++noise) 312 | switch (optionMap["method"].as()) { 313 | case 0: 314 | siciliano(J, e, NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 315 | break; 316 | case 1: 317 | hsvd(J, e, NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 318 | break; 319 | case 2: 320 | hsolver.initialize(); 321 | if (phase > 1) hsolver.Y.computeExplicitly(); 322 | if (phase > 2) hsolver.computeSolution(); 323 | break; 324 | case 3: 325 | hcod(J, e, NB_STAGE, RANKFREE, RANKLINKED, NR, NC); 326 | break; 327 | } 328 | 329 | gettimeofday(&t1, NULL); 330 | double time = (t1.tv_sec - t0.tv_sec) + (t1.tv_usec - t0.tv_usec) / 1.0e6; 331 | time /= optionMap["denoise"].as(); 332 | totalTime += time; 333 | 334 | datfile << NB_STAGE << " \t " << time << "\t" << seed << std::endl; 335 | } 336 | std::cout << totalTime << std::endl; 337 | } 338 | -------------------------------------------------------------------------------- /exe/conic_simplification.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Simplification and measure of cone volum. 4 | * 5 | * -------------------------------------------------------------------------- */ 6 | #define SOTH_DEBUG 7 | #define SOTH_DEBUG_MODE 45 8 | #include "soth/HCOD.hpp" 9 | #include "soth/debug.hpp" 10 | 11 | #ifndef WIN32 12 | #include 13 | #endif // WIN32 14 | 15 | #include 16 | #include 17 | #include "../unitTesting/gettimeofday.hpp" 18 | 19 | using namespace soth; 20 | using std::cerr; 21 | using std::cout; 22 | using std::endl; 23 | 24 | /* -------------------------------------------------------------------------- */ 25 | /* -------------------------------------------------------------------------- */ 26 | /* -------------------------------------------------------------------------- */ 27 | 28 | #include 29 | 30 | /* Solve an optimisation problem to check if the column of A is linked 31 | * to the rest of A and B, under the constraint of positivity of A 32 | * coefficients. Return true if the column is linked, false otherwise. */ 33 | 34 | template 35 | bool checkColumn(const Eigen::MatrixBase &A, const Eigen::MatrixBase &B, 36 | const int index, int sign = +1) { 37 | const int NC = (int)A.rows(), NA = (int)A.cols(), NB = (int)B.cols(), 38 | NX = (int)(NA + NB); 39 | 40 | std::vector J(3); 41 | std::vector b(3); 42 | 43 | /* SOTH structure construction. */ 44 | soth::HCOD hcod(NX, 3); 45 | 46 | J[0].resize(NA, NX); 47 | b[0].resize(NA); 48 | J[0].leftCols(NA).setIdentity(); 49 | J[0].rightCols(NB).fill(0); 50 | b[0].fill(soth::Bound(0, soth::Bound::BOUND_INF)); 51 | b[0][index] = soth::Bound(-10, soth::Bound::BOUND_INF); 52 | hcod.pushBackStage(J[0], b[0]); 53 | 54 | J[1].resize(NC, NA + NB); 55 | b[1].resize(NC); 56 | J[1].leftCols(NA) = A; 57 | J[1].rightCols(NB) = B; 58 | b[1].fill(0); 59 | hcod.pushBackStage(J[1], b[1]); 60 | 61 | J[2].resize(1, NA + NB); 62 | b[2].resize(1); 63 | J[2].fill(0); 64 | J[2](0, index) = 1; 65 | b[2].fill(-sign); 66 | hcod.pushBackStage(J[2], b[2]); 67 | 68 | hcod.setNameByOrder("stage_"); 69 | 70 | hcod.useDamp(false); 71 | hcod.setInitialActiveSet(); 72 | 73 | VectorXd solution(NX); 74 | hcod.activeSearch(solution); 75 | if (sotDEBUGFLOW.outputbuffer.good()) hcod.show(sotDEBUGFLOW.outputbuffer); 76 | // cout << "x = " << (MATLAB)solution << endl; 77 | // cout << "actset = "; hcod.showActiveSet(std::cout); 78 | // cout << "res = " << solution[index]+1 << endl; 79 | 80 | cout << "Checking const " << (sign > 0 ? "+" : "-") << index << "... "; 81 | if (std::abs(solution[index] + sign) < 1e-6) 82 | cout << " \tuseless" << endl; 83 | else 84 | cout << " \tneeded" << endl; 85 | 86 | return std::abs(solution[index] + sign) < 1e-6; 87 | } 88 | 89 | typedef SubMatrix::ColIndices Indirect; 90 | 91 | int checkAndModify(SubMatrix &A, 92 | SubMatrix &B, int index) { 93 | // cout << "A = " << (MATLAB)A << endl; 94 | // cout << "B = " << (MATLAB)B << endl; 95 | 96 | if (checkColumn(A, B, index, +1)) { 97 | A.removeCol(index); 98 | cout << "remove ... " << endl; 99 | return 0; 100 | } 101 | if (checkColumn(A, B, index, -1)) { 102 | int ref = (int)A.removeCol(index); 103 | B.pushColFront(ref); 104 | cout << "transfert ... " << endl; 105 | return 0; 106 | } 107 | 108 | return 1; 109 | } 110 | 111 | /* Check if all AB can be expressed as X (ie conservative solution). */ 112 | bool checkOut(const MatrixXd &X, const SubMatrix &A, 113 | const SubMatrix &B) { 114 | /* shoot ab = rand, with a>0 and target=AB*ab 115 | * solve X*x=target, with x>0. */ 116 | 117 | const int NC = (int)A.rows(), NA = (int)A.cols(), NB = (int)B.cols(), 118 | NX = (int)X.cols(); 119 | VectorXd ab = VectorXd::Random(NA + NB); 120 | ab.head(NA) += VectorXd::Ones(NA); 121 | ab.head(NA) /= 2; 122 | VectorXd target = A * ab.head(NA) + B * ab.tail(NB); 123 | cout << "ab = " << (MATLAB)ab << endl; 124 | cout << "target = " << (MATLAB)target << endl; 125 | 126 | std::vector J(2); 127 | std::vector b(2); 128 | 129 | /* SOTH structure construction. */ 130 | soth::HCOD hcod(NX, 2); 131 | 132 | J[0].resize(NX, NX); 133 | b[0].resize(NX); 134 | J[0].setIdentity(); 135 | b[0].fill(soth::Bound(0, soth::Bound::BOUND_INF)); 136 | hcod.pushBackStage(J[0], b[0]); 137 | 138 | J[1].resize(NC, NX); 139 | b[1].resize(NC); 140 | J[1] = X; 141 | for (int i = 0; i < NC; ++i) b[1][i] = target[i]; 142 | hcod.pushBackStage(J[1], b[1]); 143 | 144 | hcod.setNameByOrder("stage_"); 145 | 146 | hcod.useDamp(false); 147 | hcod.setInitialActiveSet(); 148 | 149 | VectorXd solution(NX); 150 | hcod.activeSearch(solution); 151 | if (sotDEBUGFLOW.outputbuffer.good()) hcod.show(sotDEBUGFLOW.outputbuffer); 152 | 153 | cout << "ab = " << (MATLAB)solution << std::endl; 154 | cout << "res = " << (MATLAB)(VectorXd)(X * solution - target) << std::endl; 155 | 156 | return (X * solution - target).norm() < 1e-6; 157 | } 158 | 159 | /* Check if all X can be expressed as AB. */ 160 | bool checkIn(const MatrixXd &X, const SubMatrix &A, 161 | const SubMatrix &B) { 162 | /* shoot x = rand>0, and target=X*x 163 | * solve Aa+Bb = x, with a>0. */ 164 | 165 | const int NC = (int)A.rows(), NA = (int)A.cols(), NB = (int)B.cols(), 166 | NX = (int)X.cols(); 167 | VectorXd x = VectorXd::Random(NX); 168 | x += VectorXd::Ones(NX); 169 | x /= 2; 170 | VectorXd target = X * x; 171 | cout << "x = " << (MATLAB)x << endl; 172 | 173 | std::vector J(2); 174 | std::vector b(2); 175 | 176 | /* SOTH structure construction. */ 177 | soth::HCOD hcod(NA + NB, 2); 178 | 179 | J[0].resize(NA, NA + NB); 180 | b[0].resize(NA); 181 | J[0].leftCols(NA).setIdentity(); 182 | J[0].rightCols(NB).fill(0); 183 | b[0].fill(soth::Bound(0, soth::Bound::BOUND_INF)); 184 | hcod.pushBackStage(J[0], b[0]); 185 | 186 | J[1].resize(NC, NA + NB); 187 | b[1].resize(NC); 188 | J[1].leftCols(NA) = A; 189 | J[1].rightCols(NB) = B; 190 | for (int i = 0; i < NC; ++i) b[1][i] = target[i]; 191 | hcod.pushBackStage(J[1], b[1]); 192 | 193 | hcod.setNameByOrder("stage_"); 194 | 195 | hcod.useDamp(false); 196 | hcod.setInitialActiveSet(); 197 | 198 | VectorXd solution(NA + NB); 199 | hcod.activeSearch(solution); 200 | if (sotDEBUGFLOW.outputbuffer.good()) hcod.show(sotDEBUGFLOW.outputbuffer); 201 | 202 | cout << "ab = " << (MATLAB)solution << std::endl; 203 | cout << "res = " << (MATLAB)(VectorXd)(J[1] * solution - target) << std::endl; 204 | 205 | return (J[1] * solution - target).norm() < 1e-6; 206 | } 207 | 208 | int main(int, char **) { 209 | #ifndef NDEBUG 210 | sotDebugTrace::openFile(); 211 | #endif 212 | 213 | const int NC = 6, NX = 40; 214 | 215 | // Eigen::MatrixXd X = MatrixXd::Random(NC,NX); 216 | // Eigen::MatrixXd X(NC,NX); 217 | // X << 1.00000, 1.00000, 1.00000, 1.00000, 0.00000, 0.00000, 0.00000, 218 | // 0.00000, -1.00000, -1.00000, -1.00000, -1.00000, -0.00000, -0.00000, 219 | // -0.00000, -0.00000, 0.00000, 0.00000, 0.00000, 0.00000 220 | // , 0.00000, 0.00000, 0.00000, 0.00000, 1.00000, 1.00000, 1.00000, 1.00000, 221 | // -0.00000, -0.00000, -0.00000, -0.00000, -1.00000, -1.00000, -1.00000, 222 | // -1.00000, 0.00000, 0.00000, 0.00000, 0.00000 , 0.00000, 0.00000, 0.00000, 223 | // 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, -0.00000, -0.00000, -0.00000, 224 | // -0.00000, -0.00000, -0.00000, -0.00000, -0.00000, 1.00000, 225 | // 1.00000, 1.00000, 1.00000 , 0.00000, 0.00000, 0.00000, 0.00000, -0.10500, 226 | // -0.10500, -0.10500, -0.10500, -0.00000, -0.00000, -0.00000, -0.00000, 227 | // 0.10500, 0.10500, 0.10500, 0.10500, 0.04500, 0.04500, -0.07000, -0.07000 , 228 | // 0.10500, 0.10500, 0.10500, 0.10500, 0.00000, 0.00000, 0.00000, 0.00000, 229 | // -0.10500, -0.10500, -0.10500, -0.10500, -0.00000, -0.00000, -0.00000, 230 | // -0.00000, 0.11000, -0.08000, -0.08000, 0.11000 , -0.04500, -0.04500, 231 | // 0.07000, 0.07000, -0.11000, 0.08000, 0.08000, -0.11000, 0.04500, 0.04500, 232 | // -0.07000, -0.07000, 0.11000, -0.08000, -0.08000, 0.11000, 0.00000, 0.00000, 233 | // 0.00000, 0.00000; 234 | 235 | Eigen::MatrixXd X(NC, NX); 236 | X << 8.0902e-01, 3.0902e-01, -3.0902e-01, -8.0902e-01, -1.0000e+00, 237 | -8.0902e-01, -3.0902e-01, 3.0902e-01, 8.0902e-01, 1.0000e+00, 8.0902e-01, 238 | 3.0902e-01, -3.0902e-01, -8.0902e-01, -1.0000e+00, -8.0902e-01, 239 | -3.0902e-01, 3.0902e-01, 8.0902e-01, 1.0000e+00, 8.0902e-01, 3.0902e-01, 240 | -3.0902e-01, -8.0902e-01, -1.0000e+00, -8.0902e-01, -3.0902e-01, 241 | 3.0902e-01, 8.0902e-01, 1.0000e+00, 8.0902e-01, 3.0902e-01, -3.0902e-01, 242 | -8.0902e-01, -1.0000e+00, -8.0902e-01, -3.0902e-01, 3.0902e-01, 243 | 8.0902e-01, 1.0000e+00, 5.8779e-01, 9.5106e-01, 9.5106e-01, 5.8779e-01, 244 | 1.2246e-16, -5.8779e-01, -9.5106e-01, -9.5106e-01, -5.8779e-01, 245 | -2.4492e-16, 5.8779e-01, 9.5106e-01, 9.5106e-01, 5.8779e-01, 1.2246e-16, 246 | -5.8779e-01, -9.5106e-01, -9.5106e-01, -5.8779e-01, -2.4492e-16, 247 | 5.8779e-01, 9.5106e-01, 9.5106e-01, 5.8779e-01, 1.2246e-16, -5.8779e-01, 248 | -9.5106e-01, -9.5106e-01, -5.8779e-01, -2.4492e-16, 5.8779e-01, 249 | 9.5106e-01, 9.5106e-01, 5.8779e-01, 1.2246e-16, -5.8779e-01, -9.5106e-01, 250 | -9.5106e-01, -5.8779e-01, -2.4492e-16, 1.0000e+00, 1.0000e+00, 1.0000e+00, 251 | 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 252 | 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 253 | 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 254 | 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 255 | 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 256 | 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 1.0000e+00, 257 | 1.0000e+00, -1.6717e-02, -5.4861e-02, -5.4861e-02, -1.6717e-02, 258 | 4.5000e-02, 1.0672e-01, 1.4486e-01, 1.4486e-01, 1.0672e-01, 4.5000e-02, 259 | -1.6717e-02, -5.4861e-02, -5.4861e-02, -1.6717e-02, 4.5000e-02, 260 | 1.0672e-01, 1.4486e-01, 1.4486e-01, 1.0672e-01, 4.5000e-02, -1.3172e-01, 261 | -1.6986e-01, -1.6986e-01, -1.3172e-01, -7.0000e-02, -8.2825e-03, 262 | 2.9861e-02, 2.9861e-02, -8.2825e-03, -7.0000e-02, -1.3172e-01, 263 | -1.6986e-01, -1.6986e-01, -1.3172e-01, -7.0000e-02, -8.2825e-03, 264 | 2.9861e-02, 2.9861e-02, -8.2825e-03, -7.0000e-02, 1.9495e-01, 1.4245e-01, 265 | 7.7553e-02, 2.5053e-02, 5.0000e-03, 2.5053e-02, 7.7553e-02, 1.4245e-01, 266 | 1.9495e-01, 2.1500e-01, 4.9468e-03, -4.7553e-02, -1.1245e-01, -1.6495e-01, 267 | -1.8500e-01, -1.6495e-01, -1.1245e-01, -4.7553e-02, 4.9468e-03, 268 | 2.5000e-02, 4.9468e-03, -4.7553e-02, -1.1245e-01, -1.6495e-01, 269 | -1.8500e-01, -1.6495e-01, -1.1245e-01, -4.7553e-02, 4.9468e-03, 270 | 2.5000e-02, 1.9495e-01, 1.4245e-01, 7.7553e-02, 2.5053e-02, 5.0000e-03, 271 | 2.5053e-02, 7.7553e-02, 1.4245e-01, 1.9495e-01, 2.1500e-01, -1.0106e-01, 272 | -1.1852e-01, -9.0710e-02, -2.8251e-02, 4.5000e-02, 1.0106e-01, 1.1852e-01, 273 | 9.0710e-02, 2.8251e-02, -4.5000e-02, 1.0617e-02, 6.2179e-02, 8.9990e-02, 274 | 8.3429e-02, 4.5000e-02, -1.0617e-02, -6.2179e-02, -8.9990e-02, 275 | -8.3429e-02, -4.5000e-02, 1.0365e-01, 9.7716e-02, 5.4453e-02, -9.6084e-03, 276 | -7.0000e-02, -1.0365e-01, -9.7716e-02, -5.4453e-02, 9.6084e-03, 277 | 7.0000e-02, -8.0252e-03, -8.2985e-02, -1.2625e-01, -1.2129e-01, 278 | -7.0000e-02, 8.0252e-03, 8.2985e-02, 1.2625e-01, 1.2129e-01, 7.0000e-02; 279 | 280 | SubMatrix A(X, true); 281 | SubMatrix B(X); 282 | 283 | cout << "Ao = " << (MATLAB)A << endl; 284 | cout << "Bo = " << (MATLAB)B << endl; 285 | 286 | int ref = 0; 287 | for (int i = 0; i < NX; ++i) ref += checkAndModify(A, B, ref); 288 | 289 | cout << "A = " << (MATLAB)A << endl; 290 | cout << "B = " << (MATLAB)B << endl; 291 | 292 | // if( A.cols()>0 ) for( int i=0;i<1000;i++ ) checkIn(X,A,B); 293 | for (int i = 0; i < 1000; i++) checkOut(X, A, B); 294 | 295 | return 0; 296 | } 297 | -------------------------------------------------------------------------------- /unitTesting/tsubmatrix.cpp: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- * 2 | * 3 | * Exhaustive asserted test of the submatrix class. 4 | * 5 | * -------------------------------------------------------------------------- */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "soth/SubMatrix.hpp" 12 | 13 | using namespace Eigen; 14 | typedef Matrix IndexType; 15 | 16 | void testPermutedBasic() { 17 | MatrixXi m(4, 5); 18 | Map map(m.data(), m.size(), 1); 19 | map = VectorXi::LinSpaced(m.size(), 0, (int)m.size() - 1); 20 | std::cout << "original matrix:" << std::endl << std::endl; 21 | std::cout << m << std::endl; 22 | 23 | SubMatrix p(m, true, true); 24 | std::cout << "creating a permuted matrix with default permutation" 25 | << std::endl; 26 | std::cout << p << std::endl << std::endl; 27 | 28 | std::cout << "permuting row 1 and 2" << std::endl; 29 | p.permuteRows(1, 2); 30 | std::cout << p << std::endl << std::endl; 31 | assert(p(1, 1) == 6); 32 | 33 | std::cout << "permuting col 2 and 4 then 1 and 4" << std::endl; 34 | p.permuteCols(2, 4); 35 | p.permuteCols(1, 4); 36 | std::cout << "col permutation indices are " << p.getColIndices().transpose() 37 | << " and the permuted matrix is" << std::endl; 38 | std::cout << p << std::endl << std::endl; 39 | assert(p(1, 1) == 10); 40 | assert(p(1, 4) == 6); 41 | 42 | std::cout << "*****Read access******" << std::endl; 43 | std::cout << "coef (1,4) is " << p(1, 4) << std::endl; 44 | std::cout << "top right corner of size (2,2) is " << std::endl; 45 | std::cout << p.topRightCorner(2, 2) << std::endl; 46 | assert(p.topRightCorner(2, 2)(1, 0) == 14); 47 | assert(p.topRightCorner(2, 2)(0, 1) == 4); 48 | 49 | std::cout << "and a triangular lower view" << std::endl; 50 | MatrixXi pt = p.triangularView(); 51 | std::cout << pt << std::endl; 52 | assert(pt(3, 3) == 15); 53 | assert(pt(3, 4) == 0); 54 | 55 | std::cout << "*****Write access******" << std::endl; 56 | std::cout << "set coef (2,1) to -1 and col(2) to -2" << std::endl; 57 | p(2, 1) = -1; 58 | p.col(2).setConstant(-2); 59 | assert(m(1, 2) == -1); 60 | assert(m(0, 4) == -2); 61 | assert(m(3, 4) == -2); 62 | 63 | std::cout << "permuted matrix:" << std::endl; 64 | std::cout << p << std::endl; 65 | std::cout << "original matrix:" << std::endl; 66 | std::cout << m << std::endl; 67 | 68 | std::cout << std::endl << std::endl; 69 | std::cout << "----Now a row permuted only----" << std::endl; 70 | SubMatrix pr(m, true); 71 | std::cout << "creating a row permuted matrix with default permutation" 72 | << std::endl; 73 | std::cout << pr << std::endl << std::endl; 74 | 75 | std::cout << "permuting row 1 and 2" << std::endl; 76 | assert(pr(1, 1) == 5); 77 | pr.permuteRows(1, 2); 78 | std::cout << pr << std::endl << std::endl; 79 | assert(pr(1, 1) == 6); 80 | 81 | std::cout << "*****Read access******" << std::endl; 82 | std::cout << "coef (1,4) is " << pr(1, 4) << std::endl; 83 | assert(pr(1, 4) == -2); 84 | std::cout << "top left corner of size (2,2) is " << std::endl; 85 | std::cout << pr.topLeftCorner(2, 2) << std::endl; 86 | assert(pr.topLeftCorner(2, 2)(1, 0) == 2); 87 | assert(pr.topLeftCorner(2, 2)(0, 1) == 4); 88 | 89 | std::cout << "and a triangular lower view" << std::endl; 90 | pt = pr.triangularView(); 91 | std::cout << pt << std::endl; 92 | assert(pt(3, 3) == 15); 93 | assert(pt(3, 4) == 0); 94 | 95 | std::cout << std::endl << std::endl; 96 | std::cout << "----Now a col submatrix----" << std::endl; 97 | SubMatrix::ColIndices ind(3); 98 | ind << 2, 4, 1; 99 | SubMatrix pc(m); 100 | pc.setColIndices(ind); 101 | std::cout << "original matrix:" << std::endl << std::endl; 102 | std::cout << m << std::endl; 103 | std::cout << "sub matrix" << std::endl; 104 | std::cout << pc << std::endl << std::endl; 105 | assert(pc.cols() == 3); 106 | assert(pc(0, 0) == m(0, 2)); 107 | assert(pc(0, 1) == m(0, 4)); 108 | assert(pc(0, 2) == m(0, 1)); 109 | } 110 | 111 | void testSubVector() { 112 | VectorXi v = VectorXi::LinSpaced(8, 0, 7); 113 | std::cout << "original vector:" << std::endl; 114 | std::cout << v.transpose() << std::endl << std::endl; 115 | 116 | Matrix row(4); 117 | row << 2, 3, 5, 0; 118 | SubMatrix pv(v, row); 119 | std::cout << "subVector:" << std::endl; 120 | std::cout << pv.transpose() << std::endl << std::endl; 121 | assert(pv[0] == 2); 122 | assert(pv[3] == 0); 123 | 124 | std::cout << "*****Read access******" << std::endl; 125 | std::cout << "coef 2 is " << pv[2] << std::endl; 126 | assert(pv[2] == 5); 127 | 128 | std::cout << "*****Operation******" << std::endl; 129 | std::cout << "subVector:" << std::endl; 130 | VectorXi res = pv.transpose() + Vector4i::Ones().transpose(); 131 | std::cout << res << std::endl << std::endl; 132 | assert(res[1] == 4); 133 | 134 | Matrix row2(4); 135 | row2 << 7, 6, 1, 4; 136 | SubMatrix pv2(v); 137 | pv2.setRowIndices(row2); 138 | pv2 = pv; 139 | std::cout << pv2.transpose() << std::endl; 140 | std::cout << v.transpose() << std::endl; 141 | 142 | pv.popRowFront(); 143 | std::cout << pv.transpose() << std::endl; 144 | pv.pushRowFront(6); 145 | std::cout << pv.transpose() << std::endl; 146 | assert(pv[0] == 3); 147 | assert(pv[1] == 3); 148 | } 149 | 150 | void testDoublePerm() { 151 | std::cout << "\n\n **** \n **** Proxy\n **** " << std::endl; 152 | 153 | MatrixXi m(4, 5); 154 | Map map(m.data(), m.size(), 1); 155 | map = VectorXi::LinSpaced((int)m.size(), 0, (int)(m.size() - 1)); 156 | std::cout << "original matrix:" << std::endl << std::endl; 157 | std::cout << m << std::endl; 158 | 159 | Matrix idx(3); 160 | idx << 3, 1, 2; 161 | SubMatrix p(m, &idx, &idx); 162 | std::cout << "double permuted matrix:" << std::endl 163 | << "." << p << "." << std::endl; 164 | assert(p(1, 2) == 9); 165 | 166 | p.pushRowFront(p.popRowBack()); 167 | std::cout << "pass last in first:" << std::endl << p << std::endl; 168 | std::cout << "ir = [" << p.getRowIndices().transpose() << " ] " << std::endl; 169 | std::cout << "ic = [" << p.getColIndices().transpose() << " ] " << std::endl; 170 | assert(&p.getRowIndices() == &p.getColIndices()); 171 | assert(p(1, 1) == 15); 172 | assert(p(0, 2) == 6); 173 | } 174 | 175 | void speedTest() { 176 | std::cout << "\n\n **** \n **** Speed \n **** " << std::endl; 177 | int N = 1000; 178 | int n[] = {5, 10, 50, 100, 250}; 179 | for (int i = 0; i < 5; ++i) { 180 | std::cout << "size " << n[i] << std::endl; 181 | MatrixXd A = MatrixXd::Random(n[i], n[i]); 182 | MatrixXd B = MatrixXd::Random(n[i], n[i]); 183 | MatrixXd C1(n[i], n[i]); 184 | MatrixXd C2(n[i], n[i]); 185 | MatrixXd C3(n[i], n[i]); 186 | MatrixXd C4(n[i], n[i]); 187 | SubMatrix Prc(A, true, true); 188 | SubMatrix Pr(A, true); 189 | SubMatrix Pc(A, true); 190 | 191 | double dummy; 192 | clock_t start, stop; 193 | double total; 194 | 195 | dummy = 0; 196 | start = clock(); 197 | for (int j = 0; j < N; ++j) { 198 | C1.noalias() = A * B; 199 | dummy += C1(0, 0); 200 | } 201 | stop = clock(); 202 | total = static_cast((stop - start) / (CLOCKS_PER_SEC * N) * 1000); 203 | std::cout << "normal mult : " << total << "ms" << std::endl; 204 | 205 | dummy = 0; 206 | start = clock(); 207 | for (int j = 0; j < N; ++j) { 208 | C2.noalias() = Prc * B; 209 | dummy += C2(0, 0); 210 | } 211 | stop = clock(); 212 | total = static_cast((stop - start) / (CLOCKS_PER_SEC * N) * 1000); 213 | std::cout << "Perm*Normal : " << total << "ms" << std::endl; 214 | 215 | dummy = 0; 216 | start = clock(); 217 | for (int j = 0; j < N; ++j) { 218 | C3.noalias() = Pr * B; 219 | dummy += C3(0, 0); 220 | } 221 | stop = clock(); 222 | total = static_cast((stop - start) / (CLOCKS_PER_SEC * N) * 1000); 223 | std::cout << "RowPerm*Normal : " << total << "ms" << std::endl; 224 | 225 | dummy = 0; 226 | start = clock(); 227 | for (int j = 0; j < N; ++j) { 228 | C4.noalias() = Pc * B; 229 | dummy += C4(0, 0); 230 | } 231 | stop = clock(); 232 | total = static_cast((stop - start) / (CLOCKS_PER_SEC * N) * 1000); 233 | std::cout << "ColPerm*Normal : " << total << "ms" << std::endl; 234 | 235 | std::cout << std::endl; 236 | } 237 | } 238 | 239 | void testCol() { 240 | Matrix row(3); 241 | row << 2, 3, 0; 242 | MatrixXi m(4, 5); 243 | Map map(m.data(), m.size(), 1); 244 | map = VectorXi::LinSpaced((int)m.size(), 0, (int)(m.size() - 1)); 245 | 246 | MatrixXi::ColXpr m1 = m.col(1); 247 | SubMatrix m1i(m1, row); 248 | 249 | SubCol l(m, row, 1); 250 | std::cout << l << std::endl; 251 | assert((l - m1i).sum() == 0); 252 | } 253 | 254 | namespace Eigen {} 255 | 256 | void testStack() { 257 | MatrixXi m1(4, 5); 258 | Map map1(m1.data(), m1.size(), 1); 259 | map1 = VectorXi::LinSpaced((int)m1.size(), 0, (int)(m1.size() - 1)); 260 | std::cout << "m1 = " << m1 << std::endl; 261 | 262 | MatrixXi m2(2, 5); 263 | Map map2(m2.data(), m2.size(), 1); 264 | map2 = VectorXi::LinSpaced((int)m2.size(), 0, (int)(m2.size() - 1)); 265 | std::cout << "m2 = " << m2 << std::endl; 266 | 267 | StackMatrix m12(m1, m2); 268 | std::cout << "m12 = " << m12 << std::endl; 269 | 270 | assert(m1(2, 3) == m12(2, 3)); 271 | assert((m12.topRows(4) - m1).sum() == 0); 272 | assert(m2(0, 3) == m12(4, 3)); 273 | assert((m12.bottomRows(2) - m2).sum() == 0); 274 | 275 | m12(0, 2) = -2; 276 | assert(m1(0, 2) == -2); 277 | m12(5, 2) = -12; 278 | assert(m2(1, 2) == -12); 279 | assert((m12.topRows(4) - m1).sum() == 0); 280 | assert((m12.bottomRows(2) - m2).sum() == 0); 281 | 282 | /* --- */ 283 | typedef SubMatrix SubMatrixXi; 284 | 285 | #ifndef XENIAL_DETECTED 286 | Matrix row1(3); 287 | row1 << 2, 3, 0; 288 | #else 289 | Matrix row1(3); 290 | row1 << 2, 3, 0; 291 | #endif 292 | SubMatrixXi m1i(m1, row1); 293 | std::cout << "m1i = " << m1i << std::endl; 294 | #ifndef XENIAL_DETECTED 295 | Matrix row2(1); 296 | row2 << 0; 297 | #else 298 | Matrix row2(1); 299 | row2 << 0; 300 | #endif 301 | SubMatrixXi m2i(m2, row2); 302 | std::cout << "m2i = " << m2i << std::endl; 303 | 304 | StackMatrix m1i2i(m1i, m2i); 305 | std::cout << "m1i2i = " << m1i2i << std::endl; 306 | 307 | StackMatrix m12i(m1, m2i); 308 | std::cout << "m12i = " << m12i << std::endl; 309 | 310 | /* --- */ 311 | typedef SubMatrix SubVectorXd; 312 | 313 | VectorXd v1(4); 314 | Map mapv1(v1.data(), v1.size(), 1); 315 | mapv1 = VectorXd::LinSpaced((int)v1.size(), 0, (int)(v1.size() - 1)); 316 | std::cout << "v1 = " << v1 << std::endl; 317 | 318 | VectorXd v2(2); 319 | Map mapv2(v2.data(), v2.size(), 1); 320 | mapv2 = VectorXd::LinSpaced(v2.size(), 0, (int)(v2.size() - 1)); 321 | std::cout << "v2 = " << v2 << std::endl; 322 | 323 | SubVectorXd v1i(v1, row1); 324 | std::cout << "v1i = " << v1i << std::endl; 325 | SubVectorXd v2i(v2, row2); 326 | std::cout << "v2i = " << v2i << std::endl; 327 | 328 | StackMatrix v12(v1, v2); 329 | std::cout << "v12 = " << v12 << std::endl; 330 | StackMatrix v1i2i(v1i, v2i); 331 | std::cout << "v1i2i = " << v1i2i << std::endl; 332 | StackMatrix v12i(v1, v2i); 333 | std::cout << "v12i = " << v12i << std::endl; 334 | 335 | MatrixBase& v1ref = v1; 336 | StackMatrix v1r2i(v1ref, v2i); 337 | std::cout << "v1r2i = " << v1r2i << std::endl; 338 | v1r2i(4, 0) = -6; 339 | std::cout << "v12i = " << v12i << std::endl; 340 | assert((v1r2i - v12i).sum() == 0); 341 | } 342 | 343 | int main(int, char**) { 344 | // testPermutedBasic(); 345 | // testSubVector(); 346 | // testDoublePerm(); 347 | // testCol(); 348 | // if( (argc>1)&&(std::string(argv[1]) == "-speed") ) speedTest(); 349 | testStack(); 350 | 351 | std::cout << "\n\n ... Everything is working fine.\n\n\n" << std::endl; 352 | } 353 | --------------------------------------------------------------------------------