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 {
16  template <typename t_Scalar, int t_primal_size>
18  {
19  public:
20  Eigen::Matrix<t_Scalar, t_primal_size, t_primal_size> QLi_aka_J;
21  Eigen::Matrix<t_Scalar, t_primal_size, Eigen::Dynamic == t_primal_size ? Eigen::Dynamic : t_primal_size + 1> R;
23 #ifdef QPMAD_USE_HOUSEHOLDER
24  Eigen::Matrix<t_Scalar, t_primal_size, t_primal_size> householder_workspace_;
25 #endif
27 
28 
29  public:
30  template <class t_MatrixType>
31  void initialize(
32  t_MatrixType &H,
33  const SolverParameters::HessianType hessian_type,
34  const MatrixIndex primal_size,
35  const bool return_inverted_cholesky_factor)
36  {
37  primal_size_ = primal_size;
38 
40  QLi_aka_J.template triangularView<Eigen::StrictlyLower>().setZero();
41  switch (hessian_type)
42  {
45  if (return_inverted_cholesky_factor)
46  {
47  H.template triangularView<Eigen::Upper>() = QLi_aka_J.template triangularView<Eigen::Upper>();
48  }
49  break;
50 
52  QLi_aka_J.template triangularView<Eigen::Upper>() = H.template triangularView<Eigen::Upper>();
53  break;
54 
55  default:
56  QPMAD_UTILS_THROW("Unexpected Hessian type in factorization.");
57  break;
58  }
59 
60  R.resize(primal_size_, primal_size_ + 1);
62 #ifdef QPMAD_USE_HOUSEHOLDER
63  householder_workspace_.resize(primal_size_, primal_size_);
64 #endif
65  }
66 
67 
68  bool update(
69  const MatrixIndex R_col,
70 #ifdef QPMAD_USE_HOUSEHOLDER
71  const bool /*is_simple*/,
72 #else
73  const bool is_simple,
74 #endif
75  const double tolerance)
76  {
77 #ifdef QPMAD_USE_HOUSEHOLDER
78  double tau;
79  double beta;
80 
81 
82  R.col(R_col).segment(R_col, length_nonzero_head_d_ - R_col).makeHouseholderInPlace(tau, beta);
83  R(R_col, R_col) = beta;
84  QLi_aka_J.middleCols(R_col, length_nonzero_head_d_ - R_col)
85  .transpose()
86  .applyHouseholderOnTheLeft(
87  R.col(R_col).segment(R_col + 1, length_nonzero_head_d_ - R_col - 1),
88  tau,
89  householder_workspace_.data());
90  /*
91  R.col(R_col).tail(primal_size_ - R_col).makeHouseholderInPlace(tau, beta);
92  QLi_aka_J.rightCols(primal_size_ - R_col).transpose().applyHouseholderOnTheLeft(
93  R.col(R_col).tail(primal_size_ - R_col - 1), tau, householder_workspace_.data());
94  R(R_col, R_col) = beta;
95  */
96 
97 
98  return (std::abs(beta) > tolerance);
99 #else
101  if (is_simple)
102  {
103  for (MatrixIndex i = length_nonzero_head_d_ - 1; i > R_col;)
104  {
105  MatrixIndex j;
106  for (j = i - 1; (0.0 == R(j, R_col)) && (j > R_col); --j)
107  {
108  }
109  givens.computeAndApply(R(j, R_col), R(i, R_col), 0.0);
110  givens.applyColumnWise(QLi_aka_J, 0, primal_size_, j, i);
111  i = j;
112  }
113  }
114  else
115  {
116  for (MatrixIndex i = length_nonzero_head_d_ - 1; i > R_col; --i)
117  {
118  givens.computeAndApply(R(i - 1, R_col), R(i, R_col), 0.0);
119  givens.applyColumnWise(QLi_aka_J, 0, primal_size_, i - 1, i);
120  }
121  }
122 
123  return (std::abs(R(R_col, R_col)) > tolerance);
124 #endif
125  }
126 
127 
128  void downdate(const MatrixIndex R_col_index, const MatrixIndex R_cols)
129  {
131  for (MatrixIndex i = R_col_index + 1; i < R_cols; ++i)
132  {
133  givens.computeAndApply(R(i - 1, i), R(i, i), 0.0);
134  givens.applyColumnWise(QLi_aka_J, 0, primal_size_, i - 1, i);
135  // 'R_cols+1' -- update 'd' as well
136  givens.applyRowWise(R, i + 1, R_cols + 1, i - 1, i);
137 
138  R.col(i - 1).segment(0, i) = R.col(i).segment(0, i);
139  }
140  // vector 'd'
141  R.col(R_cols - 1) = R.col(R_cols);
142  }
143 
144 
145  template <class t_VectorType>
147  t_VectorType &step_direction,
148  const MatrixIndex simple_bound_index,
149  const MatrixIndex active_set_size)
150  {
151  // vector 'd'
152  R.col(active_set_size) = QLi_aka_J.row(simple_bound_index).transpose();
153 
154  computePrimalStepDirection(step_direction, active_set_size);
155  }
156 
157 
158  template <class t_VectorType, class t_RowVectorType>
160  t_VectorType &step_direction,
161  const t_RowVectorType &ctr,
162  const MatrixIndex active_set_size)
163  {
164  // vector 'd'
165  R.col(active_set_size).noalias() = QLi_aka_J.transpose() * ctr.transpose();
166 
167  computePrimalStepDirection(step_direction, active_set_size);
168  }
169 
170 
171  template <class t_VectorType0, class t_ActiveSet>
172  void computeInequalityPrimalStep(t_VectorType0 &primal_step_direction, const t_ActiveSet &active_set)
173  {
174  computePrimalStepDirection(primal_step_direction, active_set.size_);
175  }
176 
177 
178 
179  template <class t_VectorType, class t_MatrixType, class t_ActiveSet>
181  t_VectorType &dual_step_direction,
182  const ChosenConstraint &chosen_ctr,
183  const t_MatrixType &A,
184  const t_ActiveSet &active_set)
185  {
186  if (chosen_ctr.is_simple_)
187  {
188  if (chosen_ctr.is_lower_)
189  {
190  R.col(active_set.size_) = -QLi_aka_J.row(chosen_ctr.index_).transpose();
191  }
192  else
193  {
194  R.col(active_set.size_) = QLi_aka_J.row(chosen_ctr.index_).transpose();
195  }
196  for (length_nonzero_head_d_ = primal_size_ - 1; (0.0 == R(length_nonzero_head_d_, active_set.size_))
197  && (length_nonzero_head_d_ > active_set.size_);
199  {
200  }
202  }
203  else
204  {
205  if (chosen_ctr.is_lower_)
206  {
207  R.col(active_set.size_).noalias() =
208  -QLi_aka_J.transpose() * A.row(chosen_ctr.general_constraint_index_).transpose();
209  }
210  else
211  {
212  R.col(active_set.size_).noalias() =
213  QLi_aka_J.transpose() * A.row(chosen_ctr.general_constraint_index_).transpose();
214  }
216  }
217 
218  computeDualStepDirection(dual_step_direction, active_set);
219  }
220 
221 
222  template <class t_VectorType0, class t_VectorType1, class t_ActiveSet>
224  t_VectorType0 &primal_step_direction,
225  t_VectorType1 &dual_step_direction,
226  const t_ActiveSet &active_set)
227  {
228  primal_step_direction.noalias() -= QLi_aka_J.col(active_set.size_) * R(active_set.size_, active_set.size_);
229  computeDualStepDirection(dual_step_direction, active_set);
230  }
231 
232 
233  template <class t_VectorType0, class t_VectorType1, class t_ActiveSet>
235  t_VectorType0 &primal_step_direction,
236  t_VectorType1 &dual_step_direction,
237  const t_ActiveSet &active_set)
238  {
239  primal_step_direction.noalias() = -QLi_aka_J.col(active_set.size_) * R(active_set.size_, active_set.size_);
240  computeDualStepDirection(dual_step_direction, active_set);
241  }
242 
243 
244  private:
245  template <class t_VectorType>
246  void computePrimalStepDirection(t_VectorType &step_direction, const MatrixIndex active_set_size)
247  {
248  step_direction.noalias() =
249  -QLi_aka_J.middleCols(active_set_size, length_nonzero_head_d_ - active_set_size)
250  * R.col(active_set_size).segment(active_set_size, length_nonzero_head_d_ - active_set_size);
251  }
252 
253 
254  template <class t_VectorType, class t_ActiveSet>
255  void computeDualStepDirection(t_VectorType &step_direction, const t_ActiveSet &active_set)
256  {
257  step_direction.segment(active_set.num_equalities_, active_set.num_inequalities_).noalias() =
258  -R.block(active_set.num_equalities_,
259  active_set.num_equalities_,
260  active_set.num_inequalities_,
261  active_set.num_inequalities_)
262  .template triangularView<Eigen::Upper>()
263  .solve(R.col(active_set.size_)
264  .segment(active_set.num_equalities_, active_set.num_inequalities_));
265  }
266  };
267 } // namespace qpmad
Represents Givens rotation.
Definition: givens.h:38
#define QPMAD_UTILS_THROW(s)
void applyColumnWise(t_MatrixType &M, const int start, const int end, const int column_1, const int column_2) const
Definition: givens.h:116
void computeInequalityDualStep(t_VectorType &dual_step_direction, const ChosenConstraint &chosen_ctr, const t_MatrixType &A, const t_ActiveSet &active_set)
Eigen::Matrix< t_Scalar, t_primal_size, t_primal_size > QLi_aka_J
MatrixIndex general_constraint_index_
void updateStepsAfterPartialStep(t_VectorType0 &primal_step_direction, t_VectorType1 &dual_step_direction, const t_ActiveSet &active_set)
EIGEN_DEFAULT_DENSE_INDEX_TYPE MatrixIndex
Definition: common.h:32
void computeEqualityPrimalStep(t_VectorType &step_direction, const t_RowVectorType &ctr, const MatrixIndex active_set_size)
void computeEqualityPrimalStep(t_VectorType &step_direction, const MatrixIndex simple_bound_index, 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:136
void computePrimalStepDirection(t_VectorType &step_direction, const MatrixIndex active_set_size)
void computeDualStepDirection(t_VectorType &step_direction, const t_ActiveSet &active_set)
static void compute(t_OutputMatrixType &U_inverse, const t_InputMatrixType &L)
Definition: inverse.h:20
Type computeAndApply(t_Scalar &a, t_Scalar &b, const t_Scalar eps)
Definition: givens.h:50
Eigen::Matrix< t_Scalar, t_primal_size, Eigen::Dynamic==t_primal_size ? Eigen::Dynamic :t_primal_size+1 > R
void downdate(const MatrixIndex R_col_index, const MatrixIndex R_cols)
bool update(const MatrixIndex R_col, const bool is_simple, const double tolerance)
void updateStepsAfterPureDualStep(t_VectorType0 &primal_step_direction, t_VectorType1 &dual_step_direction, const t_ActiveSet &active_set)
void computeInequalityPrimalStep(t_VectorType0 &primal_step_direction, const t_ActiveSet &active_set)
void initialize(t_MatrixType &H, const SolverParameters::HessianType hessian_type, const MatrixIndex primal_size, const bool return_inverted_cholesky_factor)