36 #ifndef VIGRA_QUADPROG_HXX
37 #define VIGRA_QUADPROG_HXX
40 #include "mathutil.hxx"
42 #include "linear_solve.hxx"
43 #include "numerictraits.hxx"
44 #include "array_vector.hxx"
50 template <
class T,
class C1,
class C2,
class C3>
51 bool quadprogAddConstraint(MultiArrayView<2, T, C1> & R, MultiArrayView<2, T, C2> & J, MultiArrayView<2, T, C3> & d,
52 int activeConstraintCount,
double& R_norm)
54 typedef typename MultiArrayShape<2>::type Shape;
56 linalg::detail::qrGivensStepImpl(0,
subVector(d, activeConstraintCount, n),
57 J.subarray(Shape(activeConstraintCount,0), Shape(n,n)));
58 if (
abs(d(activeConstraintCount,0)) <= NumericTraits<T>::epsilon() * R_norm)
60 R_norm = std::max<T>(R_norm,
abs(d(activeConstraintCount,0)));
62 ++activeConstraintCount;
64 columnVector(R, Shape(0, activeConstraintCount - 1), activeConstraintCount) =
subVector(d, 0, activeConstraintCount);
68 template <
class T,
class C1,
class C2,
class C3>
69 void quadprogDeleteConstraint(MultiArrayView<2, T, C1> & R, MultiArrayView<2, T, C2> & J, MultiArrayView<2, T, C3> & u,
70 int activeConstraintCount,
int constraintToBeRemoved)
72 typedef typename MultiArrayShape<2>::type Shape;
74 int newActiveConstraintCount = activeConstraintCount - 1;
76 if(constraintToBeRemoved == newActiveConstraintCount)
79 std::swap(u(constraintToBeRemoved,0), u(newActiveConstraintCount,0));
81 linalg::detail::qrGivensStepImpl(0, R.subarray(Shape(constraintToBeRemoved, constraintToBeRemoved),
82 Shape(newActiveConstraintCount,newActiveConstraintCount)),
83 J.subarray(Shape(constraintToBeRemoved, 0),
84 Shape(newActiveConstraintCount,newActiveConstraintCount)));
168 template <
class T,
class C1,
class C2,
class C3,
class C4,
class C5,
class C6,
class C7>
175 using namespace linalg;
181 constraintCount = me + mi;
184 "quadraticProgramming(): Matrix shape mismatch between G and g.");
185 vigra_precondition(
rowCount(x) == n,
186 "quadraticProgramming(): Output vector x has illegal shape.");
189 "quadraticProgramming(): Matrix CE has illegal shape.");
192 "quadraticProgramming(): Matrix CI has illegal shape.");
194 Matrix<T> J = identityMatrix<T>(n);
196 Matrix<T> L(G.
shape());
204 T f_value = 0.5 *
dot(g, x);
206 T epsilonZ = NumericTraits<T>::epsilon() *
sq(J.norm(0)),
207 inf = std::numeric_limits<T>::infinity();
209 Matrix<T> R(n, n), r(constraintCount, 1), u(constraintCount,1);
210 T R_norm = NumericTraits<T>::one();
213 for (
int i=0; i < me; ++i)
224 : (-
dot(np, x) + ce(i,0)) /
dot(z, np);
230 f_value += 0.5 *
sq(step) *
dot(z, np);
232 vigra_precondition(vigra::detail::quadprogAddConstraint(R, J, d, i, R_norm),
233 "quadraticProgramming(): Equality constraints are linearly dependent.");
235 int activeConstraintCount = me;
239 for (
int i = 0; i < mi; ++i)
242 int constraintToBeAdded = 0;
244 for (
int i = activeConstraintCount-me; i < mi; ++i)
246 T s =
dot(
rowVector(CI, activeSet[i]), x) - ci(activeSet[i], 0);
250 constraintToBeAdded = i;
254 int iter = 0, maxIter = 10*mi;
255 while(iter++ < maxIter)
263 Matrix<T> z =
transpose(J).subarray(Shape(0, activeConstraintCount), Shape(n,n))*
subVector(d, activeConstraintCount, n);
278 int constraintToBeRemoved = 0;
279 for (
int k = me; k < activeConstraintCount; ++k)
283 if (u(k,0) / r(k,0) < dualStep)
285 dualStep = u(k,0) / r(k,0);
286 constraintToBeRemoved = k;
292 T step = std::min(dualStep, primalStep);
301 if (primalStep == inf)
304 subVector(u, 0, activeConstraintCount) -= step *
subVector(r, 0, activeConstraintCount);
305 vigra::detail::quadprogDeleteConstraint(R, J, u, activeConstraintCount, constraintToBeRemoved);
306 --activeConstraintCount;
307 std::swap(activeSet[constraintToBeRemoved-me], activeSet[activeConstraintCount-me]);
314 f_value += 0.5 *
sq(step) *
dot(z, np);
316 subVector(u, 0, activeConstraintCount) -= step *
subVector(r, 0, activeConstraintCount);
317 u(activeConstraintCount,0) = step;
319 if (step == primalStep)
322 vigra::detail::quadprogAddConstraint(R, J, d, activeConstraintCount, R_norm);
323 std::swap(activeSet[constraintToBeAdded], activeSet[activeConstraintCount-me]);
324 ++activeConstraintCount;
329 vigra::detail::quadprogDeleteConstraint(R, J, u, activeConstraintCount, constraintToBeRemoved);
330 --activeConstraintCount;
331 std::swap(activeSet[constraintToBeRemoved-me], activeSet[activeConstraintCount-me]);
336 for (
int i = activeConstraintCount-me; i < mi; ++i)
339 T s =
dot(
rowVector(CI, activeSet[i]), x) - ci(activeSet[i], 0);
343 constraintToBeAdded = i;
MultiArrayView< 2, T, C > columnVector(MultiArrayView< 2, T, C > const &m, MultiArrayIndex d)
Definition: matrix.hxx:725
MultiArrayIndex rowCount(const MultiArrayView< 2, T, C > &x)
Definition: matrix.hxx:669
void transpose(const MultiArrayView< 2, T, C1 > &v, MultiArrayView< 2, T, C2 > &r)
Definition: matrix.hxx:963
bool linearSolveLowerTriangular(const MultiArrayView< 2, T, C1 > &l, const MultiArrayView< 2, T, C2 > &b, MultiArrayView< 2, T, C3 > x)
Definition: linear_solve.hxx:1064
const difference_type & shape() const
Definition: multi_array.hxx:1551
void choleskySolve(MultiArrayView< 2, T, C1 > const &L, MultiArrayView< 2, T, C2 > const &b, MultiArrayView< 2, T, C3 > &x)
Definition: linear_solve.hxx:1114
Definition: accessor.hxx:43
linalg::TemporaryMatrix< T > sq(MultiArrayView< 2, T, C > const &v)
Matrix< T, ALLLOC >::SquaredNormType squaredNorm(const Matrix< T, ALLLOC > &a)
MultiArrayView< 2, T, C > subVector(MultiArrayView< 2, T, C > const &m, int first, int end)
Definition: matrix.hxx:759
Class for fixed size vectors.This class contains an array of size SIZE of the specified VALUETYPE...
Definition: accessor.hxx:940
NormTraits< T >::SquaredNormType dot(const MultiArrayView< 2, T, C1 > &x, const MultiArrayView< 2, T, C2 > &y)
Definition: matrix.hxx:1340
MultiArrayView< 2, T, C > rowVector(MultiArrayView< 2, T, C > const &m, MultiArrayIndex d)
Definition: matrix.hxx:695
MultiArrayIndex columnCount(const MultiArrayView< 2, T, C > &x)
Definition: matrix.hxx:682
FFTWComplex< R >::NormType abs(const FFTWComplex< R > &a)
absolute value (= magnitude)
Definition: fftw3.hxx:1002
bool choleskyDecomposition(MultiArrayView< 2, T, C1 > const &A, MultiArrayView< 2, T, C2 > &L)
Definition: linear_solve.hxx:907
bool linearSolveUpperTriangular(const MultiArrayView< 2, T, C1 > &r, const MultiArrayView< 2, T, C2 > &b, MultiArrayView< 2, T, C3 > x)
Definition: linear_solve.hxx:1014
T quadraticProgramming(MultiArrayView< 2, T, C1 > const &G, MultiArrayView< 2, T, C2 > const &g, MultiArrayView< 2, T, C3 > const &CE, MultiArrayView< 2, T, C4 > const &ce, MultiArrayView< 2, T, C5 > const &CI, MultiArrayView< 2, T, C6 > const &ci, MultiArrayView< 2, T, C7 > &x)
Definition: quadprog.hxx:170