1 from pinocchio.utils
import zero, eye
3 import numpy.linalg
as npl
6 This file implements a sparse linear problem (quadric cost, linear constraints -- LCQP)
7 where the decision variables are denoted by x=(x1 ... xn), n being the number of factors.
8 The problem can be written:
9 min Sum_i=1^p || A_i x - b_i ||^2
12 so that forall j=1:q C_j x = d_i
14 Matrices A_i and C_j are block sparse, i.e. they are acting only on some (few) of the variables
17 The file implements the main class FactorGraph, which stores the LCQP problem and solve it.
18 It also provides a secondary class Factor, used to set up FactorGraph
24 A factor is a part of a linear constraint corresponding either a cost ||A x - b|| or
26 In both cases, we have Ax = sum A_i x_i, where some A_i are null. One object of class
27 Factor stores one of the A_i, along with the correspond <i> index. It is simply a pair
30 This class is used as a arguments of some of the setup functions of FactorGraph.
33 def __init__(self, index, matrix):
34 self.
indexindex = index
40 The class FactorGraph stores a block-sparse linear-constrained quadratic program (LCQP)
41 of variable x=(x1...xn). The size of the problem is set up at construction of the object.
42 Methods add_factor() and add_factor_constraint() are used to set up the problem.
43 Method solve() is used to compute the solution to the problem.
48 Initialize a QP sparse problem as min || A x - b || so that C x = d
49 where x = (x1, .., xn), and dim(xi) = variableSize and n = nbVariables
50 After construction, A, b, C and d are allocated and set to 0.
52 self.
nxnx = variableSize
54 self.
AA = zero([0, self.
NN * self.
nxnx])
56 self.
CC = zero([0, self.
NN * self.
nxnx])
61 Internal function: not designed to be called by the user.
62 Create a factor matrix [ A1 0 A2 0 A3 ... ] where the Ai's are placed at
63 the indexes of the factors.
65 assert len(factors) > 0
66 nr = factors[0].matrix.shape[0]
67 nc = self.
nxnx * self.
NN
71 for factor
in factors:
72 assert factor.matrix.shape == (nr, self.
nxnx)
73 A[:, self.
nxnx * factor.index : self.
nxnx * (factor.index + 1)] = factor.matrix
78 Add a factor || sum_{i} factor[i].matrix * x_{factor[i].index} - reference ||
83 self.
bb = np.vstack([self.
bb, reference])
87 Add a factor sum_{i} factor[i].matrix * x_{factor[i].index} = reference
92 self.
dd = np.vstack([self.
dd, reference])
96 Implement a LCQP solver, with numerical threshold eps.
98 Cp = npl.pinv(self.
CC, eps)
100 P = eye(self.
nxnx * self.
NN) - Cp * self.
CC
101 xopt += npl.pinv(self.
AA * P, eps) * (self.
bb - self.
AA * xopt)
def solve(self, eps=1e-8)
def add_factor_constraint(self, factors, reference)
def matrix_form_factor(self, factors)
def __init__(self, variableSize, nbVariables)
def add_factor(self, factors, reference)