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