householder_qr_export.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit 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 GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 
27 
34 #include <acado/code_generation/linear_solvers/householder_qr_export.hpp>
35 
36 using namespace std;
37 
38 BEGIN_NAMESPACE_ACADO
39 
40 //
41 // PUBLIC MEMBER FUNCTIONS:
42 //
43 
44 ExportHouseholderQR::ExportHouseholderQR( UserInteraction* _userInteraction,
45  const std::string& _commonHeaderName
46  ) : ExportLinearSolver( _userInteraction,_commonHeaderName )
47 {
48 }
49 
50 
51 ExportHouseholderQR::~ExportHouseholderQR( )
52 {
53 }
54 
55 
56 returnValue ExportHouseholderQR::getDataDeclarations( ExportStatementBlock& declarations,
57  ExportStruct dataStruct
58  ) const
59 {
60  return SUCCESSFUL_RETURN;
61 }
62 
63 
64 returnValue ExportHouseholderQR::getFunctionDeclarations( ExportStatementBlock& declarations
65  ) const
66 {
67  declarations.addDeclaration( solve );
68  declarations.addDeclaration( solveTriangular );
69  if( REUSE ) {
70  declarations.addDeclaration( solveReuse );
71  }
72 
73  return SUCCESSFUL_RETURN;
74 }
75 
76 
77 returnValue ExportHouseholderQR::getCode( ExportStatementBlock& code
78  )
79 {
80  unsigned run1, run2, run3;
81 
82  if( TRANSPOSE ) return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
83  //
84  // Solve the upper triangular system of equations:
85  //
86  for (run1 = nCols; run1 > (nCols - nBacksolves); run1--)
87  {
88  for (run2 = nCols - 1; run2 > (run1 - 1); run2--)
89  {
90  solveTriangular.addStatement(
91  b.getRow(run1 - 1) -= A.getSubMatrix((run1 - 1), run1, run2, run2 + 1) * b.getRow(run2));
92  }
93  solveTriangular <<
94  "b[" << toString((run1 - 1)) << "] = b["
95  << toString((run1 - 1)) << "]/A["
96  << toString((run1 - 1) * nCols + (run1 - 1)) << "];\n";
97  }
98  code.addFunction(solveTriangular);
99 
100  //
101  // Main solver function
102  //
103  solve.addStatement( determinant == 1.0 );
104 
105  if( UNROLLING || nRows <= 5 )
106  {
107  // Start the factorization:
108  for (run1 = 0; run1 < nCols; run1++)
109  {
110  for (run2 = run1; run2 < nRows; run2++)
111  {
112  solve.addStatement(
113  rk_temp.getCol(run2)
114  == A.getSubMatrix(run2, run2 + 1, run1,
115  run1 + 1));
116  }
117  // calculate norm:
118  solve.addStatement(rk_temp.getCol(nRows) ==
119  rk_temp.getCols(run1, nRows) * rk_temp.getTranspose().getRows(run1, nRows));
120  solve << rk_temp.getFullName() << "[" << toString(nRows) << "] = sqrt("
121  << rk_temp.getFullName() << "[" << toString(nRows)
122  << "]);\n";
123 
124  // update first element:
125  solve << rk_temp.getFullName() << "[" << toString(run1) << "] += ("
126  << rk_temp.getFullName() << "[" << toString(run1)
127  << "] < 0 ? -1 : 1)*" << rk_temp.getFullName()
128  << "[" << toString(nRows) << "];\n";
129 
130  // calculate norm:
131  solve.addStatement(rk_temp.getCol(nRows) ==
132  rk_temp.getCols(run1, nRows) * rk_temp.getTranspose().getRows(run1, nRows));
133  solve << rk_temp.getFullName() << "[" << toString(nRows) << "] = sqrt("
134  << rk_temp.getFullName() << "[" << toString(nRows)
135  << "]);\n";
136 
137  // normalization:
138  for (run2 = run1; run2 < nRows; run2++)
139  {
140  solve << rk_temp.getFullName() << "[" << toString(run2) << "] = "
141  << rk_temp.getFullName() << "[" << toString(run2)
142  << "]/" << rk_temp.getFullName() << "["
143  << toString(nRows) << "];\n";
144  }
145 
146  // update current column:
147  solve.addStatement(
148  rk_temp.getCol(nRows)
149  == rk_temp.getCols(run1, nRows)
150  * A.getSubMatrix(run1, nRows, run1,
151  run1 + 1));
152  solve << rk_temp.getFullName() << "[" << toString(nRows) << "] *= 2;\n";
153  solve.addStatement(
154  A.getSubMatrix(run1, run1 + 1, run1, run1 + 1) -=
155  rk_temp.getCol(run1) * rk_temp.getCol(nRows));
156 
157  solve.addStatement( determinant == determinant * A.getElement(run1, run1) );
158 
159  if (REUSE)
160  {
161  // replace zeros by results that can be reused:
162  for (run2 = run1; run2 < nRows - 1; run2++)
163  {
164  solve.addStatement(
165  A.getSubMatrix(run2 + 1, run2 + 2, run1, run1 + 1)
166  == rk_temp.getCol(run2));
167  }
168  }
169 
170  // update following columns:
171  for (run2 = run1 + 1; run2 < nCols; run2++)
172  {
173  solve.addStatement(
174  rk_temp.getCol(nRows)
175  == rk_temp.getCols(run1, nRows)
176  * A.getSubMatrix(run1, nRows, run2,
177  run2 + 1));
178  solve << rk_temp.getFullName() << "[" << toString(nRows) << "] *= 2;\n";
179  for (run3 = run1; run3 < nRows; run3++)
180  {
181  solve.addStatement(
182  A.getSubMatrix(run3, run3 + 1, run2, run2 + 1) -=
183  rk_temp.getCol(run3) * rk_temp.getCol(nRows));
184  }
185  }
186  // update right-hand side:
187  solve.addStatement(
188  rk_temp.getCol(nRows)
189  == rk_temp.getCols(run1, nRows)
190  * b.getRows(run1, nRows));
191  solve << rk_temp.getFullName() << "[" << toString(nRows) << "] *= 2;\n";
192  for (run3 = run1; run3 < nRows; run3++)
193  {
194  solve.addStatement( b.getRow(run3) -= rk_temp.getCol(run3) * rk_temp.getCol(nRows));
195  }
196 
197  if (REUSE)
198  {
199  // store last element to be reused:
200  solve.addStatement(
201  rk_temp.getCol(run1) == rk_temp.getCol(nRows - 1));
202  }
203  }
204  }
205  else
206  {
207  ExportIndex i( "i" );
208  ExportIndex j( "j" );
209  ExportIndex k( "k" );
210 
211  solve.addIndex( i );
212  solve.addIndex( j );
213  solve.addIndex( k );
214 
215  solve << "for( i=0; i < " << toString( nCols ) << "; i++ ) {\n";
216  solve << " for( j=i; j < " << toString( nRows ) << "; j++ ) {\n";
217  solve << " " << rk_temp.getFullName() << "[j] = A[j*" << toString( nCols ) << "+i];\n";
218  solve << " }\n";
219  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*" << rk_temp.getFullName() << "[i];\n";
220  solve << " for( j=i+1; j < " << toString( nRows ) << "; j++ ) {\n";
221  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[j]*" << rk_temp.getFullName() << "[j];\n";
222  solve << " }\n";
223  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] = sqrt(" << rk_temp.getFullName() << "[" << toString( nRows ) << "]);\n";
224  // update first element:
225  solve << " " << rk_temp.getFullName() << "[i] += (" << rk_temp.getFullName() << "[i] < 0 ? -1 : 1)*" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";
226  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*" << rk_temp.getFullName() << "[i];\n";
227  solve << " for( j=i+1; j < " << toString( nRows ) << "; j++ ) {\n";
228  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[j]*" << rk_temp.getFullName() << "[j];\n";
229  solve << " }\n";
230  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] = sqrt(" << rk_temp.getFullName() << "[" << toString( nRows ) << "]);\n";
231  solve << " for( j=i; j < " << toString( nRows ) << "; j++ ) {\n";
232  solve << " " << rk_temp.getFullName() << "[j] = " << rk_temp.getFullName() << "[j]/" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";
233  solve << " }\n";
234  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*A[i*" << toString( nCols ) << "+i];\n";
235  solve << " for( j=i+1; j < " << toString( nRows ) << "; j++ ) {\n";
236  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[j]*A[j*" << toString( nCols ) << "+i];\n";
237  solve << " }\n";
238  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] *= 2;\n";
239  solve << " A[i*" << toString( nCols ) << "+i] -= " << rk_temp.getFullName() << "[i]*" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";
240 
241  solve << " " << determinant.getFullName() << " *= " << " A[i * " << toString( nCols ) << " + i];\n";
242 
243  if( REUSE ) {
244  solve << " for( j=i; j < (" << toString( nRows ) << "-1); j++ ) {\n";
245  solve << " A[(j+1)*" << toString( nCols ) << "+i] = " << rk_temp.getFullName() << "[j];\n";
246  solve << " }\n";
247  }
248  solve << " for( j=i+1; j < " << toString( nCols ) << "; j++ ) {\n";
249  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*A[i*" << toString( nCols ) << "+j];\n";
250  solve << " for( k=i+1; k < " << toString( nRows ) << "; k++ ) {\n";
251  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[k]*A[k*" << toString( nCols ) << "+j];\n";
252  solve << " }\n";
253  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] *= 2;\n";
254  solve << " for( k=i; k < " << toString( nRows ) << "; k++ ) {\n";
255  solve << " A[k*" << toString( nCols ) << "+j] -= " << rk_temp.getFullName() << "[k]*" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";
256  solve << " }\n";
257  solve << " }\n";
258  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] = " << rk_temp.getFullName() << "[i]*b[i];\n";
259  solve << " for( k=i+1; k < " << toString( nRows ) << "; k++ ) {\n";
260  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] += " << rk_temp.getFullName() << "[k]*b[k];\n";
261  solve << " }\n";
262  solve << " " << rk_temp.getFullName() << "[" << toString( nRows ) << "] *= 2;\n";
263  solve << " for( k=i; k < " << toString( nRows ) << "; k++ ) {\n";
264  solve << " b[k] -= " << rk_temp.getFullName() << "[k]*" << rk_temp.getFullName() << "[" << toString( nRows ) << "];\n";
265  solve << " }\n";
266  if( REUSE ) {
267  solve << " " << rk_temp.getFullName() << "[i] = " << rk_temp.getFullName() << "[" << toString( nRows-1 ) << "];\n";
268  }
269  solve << "}\n";
270  }
271  solve.addLinebreak();
272 
273  solve.addFunctionCall(solveTriangular, A, b);
274  code.addFunction( solve );
275 
276  code.addLinebreak( 2 );
277  if( REUSE ) { // Also export the extra function which reuses the factorization of the matrix A
278  // update right-hand side:
279  for( run1 = 0; run1 < nCols; run1++ ) {
280  solveReuse.addStatement( rk_temp.getCol( nRows ) == A.getSubMatrix( run1+1,run1+2,run1,run1+1 )*b.getRow( run1 ) );
281  for( run2 = run1+1; run2 < (nRows-1); run2++ ) {
282  solveReuse.addStatement( rk_temp.getCol( nRows ) += A.getSubMatrix( run2+1,run2+2,run1,run1+1 )*b.getRow( run2 ) );
283  }
284  solveReuse.addStatement( rk_temp.getCol( nRows ) += rk_temp.getCol( run1 )*b.getRow( nRows-1 ) );
285  solveReuse << rk_temp.getFullName() << "[" << toString( nRows ) << "] *= 2;\n" ;
286  for( run3 = run1; run3 < (nRows-1); run3++ ) {
287  solveReuse.addStatement( b.getRow( run3 ) -= A.getSubMatrix( run3+1,run3+2,run1,run1+1 )*rk_temp.getCol( nRows ) );
288  }
289  solveReuse.addStatement( b.getRow( nRows-1 ) -= rk_temp.getCol( run1 )*rk_temp.getCol( nRows ) );
290  }
291  solveReuse.addLinebreak();
292 
293  solveReuse.addFunctionCall( solveTriangular, A, b );
294  code.addFunction( solveReuse );
295  }
296 
297  return SUCCESSFUL_RETURN;
298 }
299 
300 
301 returnValue ExportHouseholderQR::appendVariableNames( stringstream& string )
302 {
303  return SUCCESSFUL_RETURN;
304 }
305 
306 
307 returnValue ExportHouseholderQR::setup( )
308 {
309  int useOMP;
310  get(CG_USE_OPENMP, useOMP);
311 
312  if (nRightHandSides > 0)
313  return RET_NOT_IMPLEMENTED_YET;
314 
315  A = ExportVariable("A", nRows, nCols, REAL);
316  b = ExportVariable("b", nRows, 1, REAL);
317  rk_temp = ExportVariable("rk_temp", 1, nRows + 1, REAL);
318  solve = ExportFunction(getNameSolveFunction(), A, b, rk_temp);
319  solve.setReturnValue(determinant, false);
320  solve.addLinebreak( ); // FIX: TO MAKE SURE IT GETS EXPORTED
321  solveTriangular = ExportFunction( std::string( "solve_" ) + identifier + "triangular", A, b);
322  solveTriangular.addLinebreak( ); // FIX: TO MAKE SURE IT GETS EXPORTED
323 
324  if (REUSE)
325  {
326  solveReuse = ExportFunction(getNameSolveReuseFunction(), A, b, rk_temp);
327  solveReuse.addLinebreak(); // FIX: TO MAKE SURE IT GETS EXPORTED
328  }
329 
330  int unrollOpt;
331  userInteraction->get(UNROLL_LINEAR_SOLVER, unrollOpt);
332  UNROLLING = (bool) unrollOpt;
333 
334  return SUCCESSFUL_RETURN;
335 }
336 
337 
338 ExportVariable ExportHouseholderQR::getGlobalExportVariable( const uint factor ) const {
339 
340  int useOMP;
341  get(CG_USE_OPENMP, useOMP);
342  ExportStruct structWspace;
343  structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
344 
345  return ExportVariable( std::string( "rk_" ) + identifier + "temp", factor, nRows+1, REAL, structWspace );
346 }
347 
348 
349 //
350 // PROTECTED MEMBER FUNCTIONS:
351 //
352 
353 
354 
355 CLOSE_NAMESPACE_ACADO
356 
357 // end of file.
ExportVariable::getRow
ExportVariable getRow(const ExportIndex &idx) const
Definition: export_variable.cpp:395
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
ExportVariable::getTranspose
ExportVariable getTranspose() const
Definition: export_variable.cpp:390
ACADO_WORKSPACE
Definition: acado_types.hpp:805
AlgorithmicBase::userInteraction
UserInteraction * userInteraction
Definition: algorithmic_base.hpp:597
Options::get
returnValue get(OptionsName name, int &value) const
Definition: options.cpp:69
ExportHouseholderQR::getDataDeclarations
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
Definition: householder_qr_export.cpp:56
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
uint
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
std::toString
string toString(T const &value)
Definition: acado_io_utils.hpp:208
ExportVariable::getElement
ExportVariable getElement(const ExportIndex &rowIdx, const ExportIndex &colIdx) const
Definition: export_variable.cpp:434
CLOSE_NAMESPACE_ACADO
#define CLOSE_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:58
ExportHouseholderQR::setup
virtual returnValue setup()
Definition: householder_qr_export.cpp:307
ExportVariable::getSubMatrix
ExportVariable getSubMatrix(const ExportIndex &rowIdx1, const ExportIndex &rowIdx2, const ExportIndex &colIdx1, const ExportIndex &colIdx2) const
Definition: export_variable.cpp:425
ExportIndex
Defines a scalar-valued index variable to be used for exporting code.
Definition: export_index.hpp:54
ExportLinearSolver::solveTriangular
ExportFunction solveTriangular
Definition: linear_solver_export.hpp:294
ExportStruct
ExportStruct
Definition: acado_types.hpp:802
ExportVariable::getCols
ExportVariable getCols(const ExportIndex &idx1, const ExportIndex &idx2) const
Definition: export_variable.cpp:417
ExportHouseholderQR::ExportHouseholderQR
ExportHouseholderQR(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
Definition: householder_qr_export.cpp:44
ExportHouseholderQR::appendVariableNames
returnValue appendVariableNames(std::stringstream &string)
Definition: householder_qr_export.cpp:301
RET_NOT_IMPLEMENTED_YET
Definition: acado_types.hpp:848
ExportLinearSolver::getNameSolveReuseFunction
const std::string getNameSolveReuseFunction()
Definition: linear_solver_export.cpp:181
householder_qr_export.hpp
UNROLL_LINEAR_SOLVER
Definition: acado_types.hpp:349
UserInteraction
Encapsulates all user interaction for setting options, logging data and plotting results.
Definition: user_interaction.hpp:57
ExportFunction
Allows to export code of an arbitrary function.
Definition: export_function.hpp:58
ExportStatementBlock::addStatement
returnValue addStatement(const ExportStatement &_statement)
Definition: export_statement_block.cpp:91
CG_USE_OPENMP
Definition: acado_types.hpp:339
ExportHouseholderQR::getCode
virtual returnValue getCode(ExportStatementBlock &code)
Definition: householder_qr_export.cpp:77
ExportData::getFullName
std::string getFullName() const
Definition: export_data.cpp:121
REAL
Definition: acado_types.hpp:796
ExportStatementBlock::addLinebreak
returnValue addLinebreak(uint num=1)
Definition: export_statement_block.cpp:209
ExportFunction::setReturnValue
ExportFunction & setReturnValue(const ExportVariable &_functionReturnValue, bool _returnAsPointer=false)
Definition: export_function.cpp:141
ACADO_LOCAL
Definition: acado_types.hpp:808
ExportVariable::getRows
ExportVariable getRows(const ExportIndex &idx1, const ExportIndex &idx2) const
Definition: export_variable.cpp:409
ExportStatementBlock::addDeclaration
returnValue addDeclaration(const ExportVariable &_data, ExportStruct _dataStruct=ACADO_ANY)
Definition: export_statement_block.cpp:158
ExportLinearSolver::getNameSolveFunction
const std::string getNameSolveFunction()
Definition: linear_solver_export.cpp:175
BEGIN_NAMESPACE_ACADO
#define BEGIN_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:57
ExportStatementBlock::addFunction
returnValue addFunction(const ExportFunction &_function)
Definition: export_statement_block.cpp:108
ExportLinearSolver
Allows to export automatically generated algorithms for solving linear systems of specific dimensions...
Definition: linear_solver_export.hpp:62
RET_NOT_YET_IMPLEMENTED
Definition: acado_types.hpp:847
ExportStatementBlock
Allows to export code for a block of statements.
Definition: export_statement_block.hpp:63
ExportVariable::getCol
ExportVariable getCol(const ExportIndex &idx) const
Definition: export_variable.cpp:402
ExportFunction::addIndex
ExportFunction & addIndex(const ExportIndex &_index)
Definition: export_function.cpp:347
ACADOERROR
#define ACADOERROR(retval)
Definition: acado_message_handling.hpp:106
ExportHouseholderQR::getFunctionDeclarations
virtual returnValue getFunctionDeclarations(ExportStatementBlock &declarations) const
Definition: householder_qr_export.cpp:64
ExportVariable
Defines a matrix-valued variable to be used for exporting code.
Definition: export_variable.hpp:58
ExportStatementBlock::addFunctionCall
returnValue addFunctionCall(const std::string &_fName, const ExportArgument &_argument1=emptyConstExportArgument, const ExportArgument &_argument2=emptyConstExportArgument, const ExportArgument &_argument3=emptyConstExportArgument, const ExportArgument &_argument4=emptyConstExportArgument, const ExportArgument &_argument5=emptyConstExportArgument, const ExportArgument &_argument6=emptyConstExportArgument, const ExportArgument &_argument7=emptyConstExportArgument, const ExportArgument &_argument8=emptyConstExportArgument, const ExportArgument &_argument9=emptyConstExportArgument)
Definition: export_statement_block.cpp:115
ExportHouseholderQR::getGlobalExportVariable
virtual ExportVariable getGlobalExportVariable(const uint factor) const
Definition: householder_qr_export.cpp:338

acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:40