1# New functions for version 1.5 of the Python wrapper
2import cmsisdsp as dsp
3import cmsisdsp.fixedpoint as f
4import numpy as np
5import math
6import colorama
7from colorama import init,Fore, Back, Style
8from numpy.testing import assert_allclose
9
10from numpy.linalg import qr
11
12def householder(x,eps=1e-16):
13    #print(x)
14    v=np.hstack([[1],x[1:]])
15
16    alpha = x[0]
17    xnorm2=x[1:].dot(x[1:])
18    epsilon=eps
19
20    #print(sigma)
21
22    if xnorm2<=epsilon:
23       tau = 0.0
24       v = np.zeros(len(x))
25    else:
26       if np.sign(alpha) <= 0:
27          beta = math.sqrt(alpha*alpha + xnorm2)
28       else:
29          beta = -math.sqrt(alpha*alpha + xnorm2)
30
31       r =  (alpha - beta)
32       v = x / r
33       tau = (beta - alpha) / beta
34       v[0] = 1
35
36    return(v,tau)
37
38init()
39
40def printTitle(s):
41    print("\n" + Fore.GREEN + Style.BRIGHT +  s + Style.RESET_ALL)
42
43def printSubTitle(s):
44    print("\n" + Style.BRIGHT + s + Style.RESET_ALL)
45
46printTitle("Householder")
47
48VECDIM = 10
49
50a=np.random.randn(VECDIM)
51a = a / np.max(np.abs(a))
52
53# Reference
54vRef,betaRef = householder(a)
55
56printSubTitle("Householder F32")
57betaF32,vF32 = dsp.arm_householder_f32(a,dsp.DEFAULT_HOUSEHOLDER_THRESHOLD_F32)
58print(np.isclose(betaRef,betaF32,1e-6,1e-6))
59print(np.isclose(vRef,vF32,1e-6,1e-6))
60
61printSubTitle("Householder F64")
62betaF64,vF64 = dsp.arm_householder_f64(a,dsp.DEFAULT_HOUSEHOLDER_THRESHOLD_F64)
63print(np.isclose(betaRef,betaF64,1e-6,1e-6))
64print(np.isclose(vRef,vF64,1e-6,1e-6))
65
66printSubTitle("Householder Proportional F32")
67a=np.random.randn(5)
68# With the threshold defined with DEFAULT_HOUSEHOLDER_THRESHOLD_F32
69# this vector is considered as proportional to (1,0,...)
70# and thus the function will return (0,[0,...,0])
71a = a / np.max(np.abs(a)) * 1.0e-7
72resF32 = dsp.arm_householder_f32(a,dsp.DEFAULT_HOUSEHOLDER_THRESHOLD_F32)
73print(resF32)
74
75# With a smaller threshold, a computation is taking place
76resF32 = dsp.arm_householder_f32(a,0.001*dsp.DEFAULT_HOUSEHOLDER_THRESHOLD_F32)
77print(resF32)
78
79printTitle("QR decomposition")
80
81def checkOrtho(A,err=1e-10):
82    product = A.T.dot(A)
83    #print(A)
84    np.fill_diagonal(product,0)
85    #print(product)
86    print(np.max(np.abs(product)))
87    return (np.all(np.abs(product)<=err))
88
89rows = 8
90columns = 5
91
92def randomIsometry(rows,cols,rank):
93    if rank==1:
94       r=np.random.randn(rows)
95       r = Tools.normalize(r)[np.newaxis]
96       c=np.random.randn(cols)
97       c = Tools.normalize(c)[np.newaxis]
98       result=r.T.dot(c)
99    else:
100        a = np.random.randn(rows,rows)
101        b = np.random.randn(cols,cols)
102
103        diagDim = min(rows,cols)
104        d = np.zeros((rows,cols))
105
106        diag = np.ones(diagDim)
107        diag[rank:] = 0
108        np.fill_diagonal(d,diag)
109
110        qa,_ = qr(a)
111        qb,_ = qr(b)
112
113        result = qa .dot(d.dot(qb))
114    return(result)
115
116
117m = randomIsometry(rows,columns,columns-1)
118
119rows,columns = m.shape
120
121# The CMSIS-DSP C functions is requiring two temporary arrays
122# To follow the C function as closely as possible, we create
123# two arrays. Their size will be used internally by the Python
124# wrapper to allocate two temporary buffers.
125# Like that you can check you have dimensionned the arrays in the
126# right way.
127# The content of the temporary buffers is not accesible from the
128# Python API. tmpa and tmpb are not modified.
129tmpa=np.zeros(rows)
130tmpb=np.zeros(rows)
131
132
133printSubTitle("QR F64")
134
135status,r,q,tau = dsp.arm_mat_qr_f64(m,dsp.DEFAULT_HOUSEHOLDER_THRESHOLD_F64,tmpa,tmpb)
136
137# Status different from 0 if matrix dimensions are not right
138# (rows must be >= columns)
139#print(status)
140#print(q)
141#print(r)
142#print(tau)
143
144# Check that the matrix Q is orthogonal
145assert(checkOrtho(q,err=1e-14))
146
147# Remove householder vectors from R matrix
148i=1
149for c in r.T:
150    c[i:] = 0
151    i = i+1
152
153# Check that M = Q R
154newm = np.dot(q,r)
155assert_allclose(newm,m)
156
157printSubTitle("QR F32")
158
159status,r,q,tau = dsp.arm_mat_qr_f32(m,dsp.DEFAULT_HOUSEHOLDER_THRESHOLD_F32,tmpa,tmpb)
160
161# Status different from 0 if matrix dimensions are not right
162# (rows must be >= columns)
163#print(status)
164#print(q)
165#print(r)
166#print(tau)
167
168
169# Check that the matrix Q is orthogonal
170assert(checkOrtho(q,err=1.0e-6))
171
172# Remove householder vectors from R matrix
173i=1
174for c in r.T:
175    c[i:] = 0
176    i = i+1
177
178# Check that M = Q R
179newm = np.dot(q,r)
180assert_allclose(newm,m,2e-6,1e-7)