qpmad
Eigen-based C++ QP solver.
factorization_data.h
Go to the documentation of this file.
1 /**
2  @file
3  @author Alexander Sherikov
4 
5  @copyright 2017 Alexander Sherikov. Licensed under the Apache License,
6  Version 2.0. (see LICENSE or http://www.apache.org/licenses/LICENSE-2.0)
7 
8  @brief
9 */
10 
11 #pragma once
12 
13 
14 namespace qpmad
15 {
17  {
18  public:
22 #ifdef QPMAD_USE_HOUSEHOLDER
23  QPMatrix householder_workspace_;
24 #endif
26 
27 
28  public:
29  template <class t_MatrixType>
30  void initialize(const t_MatrixType &H,
31  const MatrixIndex primal_size)
32  {
33  primal_size_ = primal_size;
34 
36  QLi_aka_J.triangularView<Eigen::Lower>().setZero();
38 
39  R.resize(primal_size_, primal_size_ + 1);
41 #ifdef QPMAD_USE_HOUSEHOLDER
42  householder_workspace_.resize(primal_size_, primal_size_);
43 #endif
44  }
45 
46 
47  bool update(const MatrixIndex R_col,
48 #ifdef QPMAD_USE_HOUSEHOLDER
49  const bool /*is_simple*/,
50 #else
51  const bool is_simple,
52 #endif
53  const double tolerance)
54  {
55 #ifdef QPMAD_USE_HOUSEHOLDER
56  double tau;
57  double beta;
58 
59 
60  R.col(R_col).segment(R_col, length_nonzero_head_d_ - R_col).makeHouseholderInPlace(tau, beta);
61  R(R_col, R_col) = beta;
62  QLi_aka_J.middleCols(R_col, length_nonzero_head_d_ - R_col).transpose().applyHouseholderOnTheLeft(
63  R.col(R_col).segment(R_col+1, length_nonzero_head_d_ - R_col - 1), tau, householder_workspace_.data());
64  /*
65  R.col(R_col).tail(primal_size_ - R_col).makeHouseholderInPlace(tau, beta);
66  QLi_aka_J.rightCols(primal_size_ - R_col).transpose().applyHouseholderOnTheLeft(
67  R.col(R_col).tail(primal_size_ - R_col - 1), tau, householder_workspace_.data());
68  R(R_col, R_col) = beta;
69  */
70 
71 
72  return ( std::abs(beta) > tolerance );
73 #else
75  if (is_simple)
76  {
77  for (MatrixIndex i = length_nonzero_head_d_-1; i > R_col;)
78  {
79  MatrixIndex j;
80  for (j = i-1; (0.0 == R(j, R_col)) && (j > R_col); --j)
81  {}
82  givens.computeAndApply(R(j, R_col), R(i, R_col), 0.0);
83  givens.applyColumnWise(QLi_aka_J, 0, primal_size_, j, i);
84  i = j;
85  }
86  }
87  else
88  {
89  for (MatrixIndex i = length_nonzero_head_d_-1; i > R_col; --i)
90  {
91  givens.computeAndApply(R(i-1, R_col), R(i, R_col), 0.0);
92  givens.applyColumnWise(QLi_aka_J, 0, primal_size_, i-1, i);
93  }
94  }
95 
96  return ( std::abs(R(R_col, R_col)) > tolerance );
97 #endif
98  }
99 
100 
101  void downdate( const MatrixIndex R_col_index,
102  const MatrixIndex R_cols)
103  {
104  GivensRotation<double> givens;
105  for (MatrixIndex i = R_col_index + 1; i < R_cols; ++i)
106  {
107  givens.computeAndApply(R(i-1, i), R(i, i), 0.0);
108  givens.applyColumnWise(QLi_aka_J, 0, primal_size_, i-1, i);
109  // 'R_cols+1' -- update 'd' as well
110  givens.applyRowWise(R, i+1, R_cols+1, i-1, i);
111 
112  R.col(i-1).segment(0, i) = R.col(i).segment(0, i);
113  }
114  // vector 'd'
115  R.col(R_cols-1) = R.col(R_cols);
116  }
117 
118 
119  template<class t_VectorType>
120  void computeEqualityPrimalStep( t_VectorType & step_direction,
121  const MatrixIndex simple_bound_index,
122  const MatrixIndex active_set_size)
123  {
124  // vector 'd'
125  R.col(active_set_size) = QLi_aka_J.row(simple_bound_index).transpose();
126 
127  computePrimalStepDirection(step_direction, active_set_size);
128  }
129 
130 
131  template< class t_VectorType,
132  class t_RowVectorType>
133  void computeEqualityPrimalStep( t_VectorType & step_direction,
134  const t_RowVectorType & ctr,
135  const MatrixIndex active_set_size)
136  {
137  // vector 'd'
138  R.col(active_set_size).noalias() = QLi_aka_J.transpose() * ctr.transpose();
139 
140  computePrimalStepDirection(step_direction, active_set_size);
141  }
142 
143 
144  template<class t_VectorType0>
145  void computeInequalityPrimalStep( t_VectorType0 & primal_step_direction,
146  const ActiveSet & active_set)
147  {
148  computePrimalStepDirection(primal_step_direction, active_set.size_);
149  }
150 
151 
152 
153  template< class t_VectorType,
154  class t_MatrixType>
155  void computeInequalityDualStep( t_VectorType & dual_step_direction,
156  const ChosenConstraint & chosen_ctr,
157  const t_MatrixType & A,
158  const ActiveSet & active_set)
159  {
160  if (chosen_ctr.is_simple_)
161  {
162  if (chosen_ctr.is_lower_)
163  {
164  R.col(active_set.size_) = - QLi_aka_J.row(chosen_ctr.index_).transpose();
165  }
166  else
167  {
168  R.col(active_set.size_) = QLi_aka_J.row(chosen_ctr.index_).transpose();
169  }
171  (0.0 == R(length_nonzero_head_d_,active_set.size_)) && (length_nonzero_head_d_ > active_set.size_);
173  {}
175  }
176  else
177  {
178  if (chosen_ctr.is_lower_)
179  {
180  R.col(active_set.size_).noalias() =
181  - QLi_aka_J.transpose() * A.row(chosen_ctr.general_constraint_index_).transpose();
182  }
183  else
184  {
185  R.col(active_set.size_).noalias() =
186  QLi_aka_J.transpose() * A.row(chosen_ctr.general_constraint_index_).transpose();
187  }
189  }
190 
191  computeDualStepDirection(dual_step_direction, active_set);
192  }
193 
194 
195  template< class t_VectorType0,
196  class t_VectorType1>
197  void updateStepsAfterPartialStep( t_VectorType0 & primal_step_direction,
198  t_VectorType1 & dual_step_direction,
199  const ActiveSet & active_set)
200  {
201  primal_step_direction.noalias() -= QLi_aka_J.col(active_set.size_) * R(active_set.size_, active_set.size_);
202  computeDualStepDirection(dual_step_direction, active_set);
203  }
204 
205 
206  template< class t_VectorType0,
207  class t_VectorType1>
208  void updateStepsAfterPureDualStep( t_VectorType0 & primal_step_direction,
209  t_VectorType1 & dual_step_direction,
210  const ActiveSet & active_set)
211  {
212  primal_step_direction.noalias() = -QLi_aka_J.col(active_set.size_) * R(active_set.size_, active_set.size_);
213  computeDualStepDirection(dual_step_direction, active_set);
214  }
215 
216 
217  private:
218  template<class t_VectorType>
219  void computePrimalStepDirection(t_VectorType & step_direction,
220  const MatrixIndex active_set_size)
221  {
222  step_direction.noalias() =
223  - QLi_aka_J.middleCols(active_set_size, length_nonzero_head_d_ - active_set_size)
224  * R.col(active_set_size).segment(active_set_size, length_nonzero_head_d_ - active_set_size);
225  }
226 
227 
228  template<class t_VectorType>
229  void computeDualStepDirection( t_VectorType & step_direction,
230  const ActiveSet &active_set)
231  {
232  step_direction.segment(active_set.num_equalities_, active_set.num_inequalities_).noalias() =
233  - R.block(active_set.num_equalities_,
234  active_set.num_equalities_,
235  active_set.num_inequalities_,
236  active_set.num_inequalities_).triangularView<Eigen::Upper>().solve(
237  R.col(active_set.size_).segment(
238  active_set.num_equalities_,
239  active_set.num_inequalities_));
240  }
241  };
242 }
Represents Givens rotation.
Definition: givens.h:38
void applyColumnWise(t_MatrixType &M, const int start, const int end, const int column_1, const int column_2) const
Definition: givens.h:116
MatrixIndex general_constraint_index_
void computeDualStepDirection(t_VectorType &step_direction, const ActiveSet &active_set)
int MatrixIndex
Definition: common.h:32
MatrixIndex num_inequalities_
Definition: active_set.h:22
void computePrimalStepDirection(t_VectorType &step_direction, const MatrixIndex active_set_size)
void applyRowWise(t_MatrixType &M, const int start, const int end, const int row_1, const int row_2) const
Definition: givens.h:140
void downdate(const MatrixIndex R_col_index, const MatrixIndex R_cols)
void updateStepsAfterPartialStep(t_VectorType0 &primal_step_direction, t_VectorType1 &dual_step_direction, const ActiveSet &active_set)
void computeEqualityPrimalStep(t_VectorType &step_direction, const t_RowVectorType &ctr, const MatrixIndex active_set_size)
static void compute(t_OutputMatrixType &U_inverse, const t_InputMatrixType &L)
Definition: inverse.h:21
void updateStepsAfterPureDualStep(t_VectorType0 &primal_step_direction, t_VectorType1 &dual_step_direction, const ActiveSet &active_set)
Type computeAndApply(t_Scalar &a, t_Scalar &b, const t_Scalar eps)
Definition: givens.h:50
bool update(const MatrixIndex R_col, const bool is_simple, const double tolerance)
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > QPMatrix
Definition: common.h:34
MatrixIndex num_equalities_
Definition: active_set.h:21
void initialize(const t_MatrixType &H, const MatrixIndex primal_size)
void computeEqualityPrimalStep(t_VectorType &step_direction, const MatrixIndex simple_bound_index, const MatrixIndex active_set_size)
void computeInequalityDualStep(t_VectorType &dual_step_direction, const ChosenConstraint &chosen_ctr, const t_MatrixType &A, const ActiveSet &active_set)
void computeInequalityPrimalStep(t_VectorType0 &primal_step_direction, const ActiveSet &active_set)
MatrixIndex size_
Definition: active_set.h:20