Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 9049e209feadaab0336e96bf9c1d861f60cc5aa9 / . / tests / lin_alg / test_lin_alg.h
blob: 735216e5004822bf5ca342f8fc6c8be3b0e04520 [file] [log] [blame]
Austin Schuh9049e202022-02-20 17:34:16 -0800[diff] [blame^]1#include <stdio.h>
2#include "osqp.h"
3#include "osqp_tester.h"
4
5#include "lin_alg/data.h"
6
7void test_constr_sparse_mat() {
8 c_float *Adns; // Conversion to dense matrix
9
10 lin_alg_sols_data *data = generate_problem_lin_alg_sols_data();
11
12 // Convert sparse to dense
13 Adns = csc_to_dns(data->test_sp_matrix_A);
14
15 // Compute norm of the elementwise difference with
16 mu_assert("Linear algebra tests: error in constructing sparse/dense matrix!",
17 vec_norm_inf_diff(Adns, data->test_sp_matrix_Adns,
18 data->test_sp_matrix_A->m *
19 data->test_sp_matrix_A->n) < TESTS_TOL);
20
21 // Free memory
22 c_free(Adns); // because of vars from file matrices.h
23 clean_problem_lin_alg_sols_data(data);
24
25}
26
27void test_vec_operations() {
28 c_float norm_inf, vecprod; // normInf;
29 c_float *ew_reciprocal;
30 c_float *add_scaled;
31 c_float *vec_ew_max_vec_test, *vec_ew_min_vec_test;
32
33 lin_alg_sols_data *data = generate_problem_lin_alg_sols_data();
34
35
36 // Add scaled
37 add_scaled = vec_copy(data->test_vec_ops_v1, data->test_vec_ops_n);
38 vec_add_scaled(add_scaled,
39 add_scaled,
40 data->test_vec_ops_v2,
41 data->test_vec_ops_n,
42 data->test_vec_ops_sc);
43 mu_assert(
44 "Linear algebra tests: error in vector operation, adding scaled vector",
45 vec_norm_inf_diff(add_scaled, data->test_vec_ops_add_scaled,
46 data->test_vec_ops_n) < TESTS_TOL);
47
48 // Norm_inf of the difference
49 mu_assert(
50 "Linear algebra tests: error in vector operation, norm_inf of difference",
51 c_absval(vec_norm_inf_diff(data->test_vec_ops_v1,
52 data->test_vec_ops_v2,
53 data->test_vec_ops_n) -
54 data->test_vec_ops_norm_inf_diff) <
55 TESTS_TOL);
56
57 // norm_inf
58 norm_inf = vec_norm_inf(data->test_vec_ops_v1, data->test_vec_ops_n);
59 mu_assert("Linear algebra tests: error in vector operation, norm_inf",
60 c_absval(norm_inf - data->test_vec_ops_norm_inf) < TESTS_TOL);
61
62 // Elementwise reciprocal
63 ew_reciprocal = (c_float *)c_malloc(data->test_vec_ops_n * sizeof(c_float));
64 vec_ew_recipr(data->test_vec_ops_v1, ew_reciprocal, data->test_vec_ops_n);
65 mu_assert(
66 "Linear algebra tests: error in vector operation, elementwise reciprocal",
67 vec_norm_inf_diff(ew_reciprocal, data->test_vec_ops_ew_reciprocal,
68 data->test_vec_ops_n) < TESTS_TOL);
69
70
71 // Vector product
72 vecprod = vec_prod(data->test_vec_ops_v1,
73 data->test_vec_ops_v2,
74 data->test_vec_ops_n);
75 mu_assert("Linear algebra tests: error in vector operation, vector product",
76 c_absval(vecprod - data->test_vec_ops_vec_prod) < TESTS_TOL);
77
78 // Elementwise maximum between two vectors
79 vec_ew_max_vec_test =
80 (c_float *)c_malloc(data->test_vec_ops_n * sizeof(c_float));
81 vec_ew_max_vec(data->test_vec_ops_v1,
82 data->test_vec_ops_v2,
83 vec_ew_max_vec_test,
84 data->test_vec_ops_n);
85 mu_assert(
86 "Linear algebra tests: error in vector operation, elementwise maximum between vectors",
87 vec_norm_inf_diff(vec_ew_max_vec_test, data->test_vec_ops_ew_max_vec,
88 data
89 ->test_vec_ops_n) < TESTS_TOL);
90
91 // Elementwise minimum between two vectors
92 vec_ew_min_vec_test =
93 (c_float *)c_malloc(data->test_vec_ops_n * sizeof(c_float));
94 vec_ew_min_vec(data->test_vec_ops_v1,
95 data->test_vec_ops_v2,
96 vec_ew_min_vec_test,
97 data->test_vec_ops_n);
98 mu_assert(
99 "Linear algebra tests: error in vector operation, elementwise minimum between vectors",
100 vec_norm_inf_diff(vec_ew_min_vec_test, data->test_vec_ops_ew_min_vec,
101 data
102 ->test_vec_ops_n) < TESTS_TOL);
103
104 // cleanup
105 c_free(add_scaled);
106 c_free(ew_reciprocal);
107 c_free(vec_ew_min_vec_test);
108 c_free(vec_ew_max_vec_test);
109 clean_problem_lin_alg_sols_data(data);
110}
111
112void test_mat_operations() {
113 csc *Ad, *dA; // Matrices used for tests
114 // csc *A_ewsq, *A_ewabs; // Matrices used for tests
115 c_int exitflag = 0;
116
117 // c_float trace, fro_sq;
118 c_float *inf_norm_cols_rows_test;
119
120
121 lin_alg_sols_data *data = generate_problem_lin_alg_sols_data();
122
123
124 // Copy matrices
125 Ad = copy_csc_mat(data->test_mat_ops_A);
126 dA = copy_csc_mat(data->test_mat_ops_A);
127
128
129 // Premultiply matrix A
130 mat_premult_diag(dA, data->test_mat_ops_d);
131 mu_assert(
132 "Linear algebra tests: error in matrix operation, premultiply diagonal",
133 is_eq_csc(dA, data->test_mat_ops_prem_diag, TESTS_TOL));
134
135
136 // Postmultiply matrix A
137 mat_postmult_diag(Ad, data->test_mat_ops_d);
138 mu_assert(
139 "Linear algebra tests: error in matrix operation, postmultiply diagonal",
140 is_eq_csc(Ad, data->test_mat_ops_postm_diag, TESTS_TOL));
141
142 // Maximum norm over columns
143 inf_norm_cols_rows_test =
144 (c_float *)c_malloc(data->test_mat_ops_n * sizeof(c_float));
145 mat_inf_norm_cols(data->test_mat_ops_A, inf_norm_cols_rows_test);
146 mu_assert(
147 "Linear algebra tests: error in matrix operation, max norm over columns",
148 vec_norm_inf_diff(inf_norm_cols_rows_test, data->test_mat_ops_inf_norm_cols,
149 data
150 ->test_mat_ops_n) < TESTS_TOL);
151
152 // Maximum norm over rows
153 mat_inf_norm_rows(data->test_mat_ops_A, inf_norm_cols_rows_test);
154 mu_assert("Linear algebra tests: error in matrix operation, max norm over rows",
155 vec_norm_inf_diff(inf_norm_cols_rows_test,
156 data->test_mat_ops_inf_norm_rows,
157 data
158 ->test_mat_ops_n) < TESTS_TOL);
159
160
161 // cleanup
162 c_free(inf_norm_cols_rows_test);
163 csc_spfree(Ad);
164 csc_spfree(dA);
165 clean_problem_lin_alg_sols_data(data);
166}
167
168void test_mat_vec_multiplication() {
169 c_float *Ax, *ATy, *Px, *Ax_cum, *ATy_cum, *Px_cum;
170
171 lin_alg_sols_data *data = generate_problem_lin_alg_sols_data();
172
173
174 // Allocate vectors
175 Ax = (c_float *)c_malloc(data->test_mat_vec_m * sizeof(c_float));
176 ATy = (c_float *)c_malloc(data->test_mat_vec_n * sizeof(c_float));
177 Px = (c_float *)c_malloc(data->test_mat_vec_n * sizeof(c_float));
178
179
180 // Matrix-vector multiplication: y = Ax
181 mat_vec(data->test_mat_vec_A, data->test_mat_vec_x, Ax, 0);
182 mu_assert(
183 "Linear algebra tests: error in matrix-vector operation, matrix-vector multiplication",
184 vec_norm_inf_diff(Ax, data->test_mat_vec_Ax,
185 data->test_mat_vec_m) < TESTS_TOL);
186
187 // Cumulative matrix-vector multiplication: y += Ax
188 Ax_cum = vec_copy(data->test_mat_vec_y, data->test_mat_vec_m);
189 mat_vec(data->test_mat_vec_A, data->test_mat_vec_x, Ax_cum, 1);
190 mu_assert(
191 "Linear algebra tests: error in matrix-vector operation, cumulative matrix-vector multiplication",
192 vec_norm_inf_diff(Ax_cum, data->test_mat_vec_Ax_cum,
193 data->test_mat_vec_m) < TESTS_TOL);
194
195 // Matrix-transpose-vector multiplication: x = A'*y
196 mat_tpose_vec(data->test_mat_vec_A, data->test_mat_vec_y, ATy, 0, 0);
197 mu_assert(
198 "Linear algebra tests: error in matrix-vector operation, matrix-transpose-vector multiplication",
199 vec_norm_inf_diff(ATy, data->test_mat_vec_ATy,
200 data->test_mat_vec_n) < TESTS_TOL);
201
202 // Cumulative matrix-transpose-vector multiplication: x += A'*y
203 ATy_cum = vec_copy(data->test_mat_vec_x, data->test_mat_vec_n);
204 mat_tpose_vec(data->test_mat_vec_A, data->test_mat_vec_y, ATy_cum, 1, 0);
205 mu_assert(
206 "Linear algebra tests: error in matrix-vector operation, cumulative matrix-transpose-vector multiplication",
207 vec_norm_inf_diff(ATy_cum, data->test_mat_vec_ATy_cum,
208 data->test_mat_vec_n) < TESTS_TOL);
209
210 // Symmetric-matrix-vector multiplication (only upper part is stored)
211 mat_vec(data->test_mat_vec_Pu, data->test_mat_vec_x, Px, 0); // upper
212 // traingular
213 // part
214 mat_tpose_vec(data->test_mat_vec_Pu, data->test_mat_vec_x, Px, 1, 1); // lower
215 // traingular
216 // part
217 // (without
218 // diagonal)
219 mu_assert(
220 "Linear algebra tests: error in matrix-vector operation, symmetric matrix-vector multiplication",
221 vec_norm_inf_diff(Px, data->test_mat_vec_Px,
222 data->test_mat_vec_n) < TESTS_TOL);
223
224
225 // Cumulative symmetric-matrix-vector multiplication
226 Px_cum = vec_copy(data->test_mat_vec_x, data->test_mat_vec_n);
227 mat_vec(data->test_mat_vec_Pu, data->test_mat_vec_x, Px_cum, 1); // upper
228 // traingular
229 // part
230 mat_tpose_vec(data->test_mat_vec_Pu, data->test_mat_vec_x, Px_cum, 1, 1); // lower
231 // traingular
232 // part
233 // (without
234 // diagonal)
235 mu_assert(
236 "Linear algebra tests: error in matrix-vector operation, cumulative symmetric matrix-vector multiplication",
237 vec_norm_inf_diff(Px_cum, data->test_mat_vec_Px_cum,
238 data->test_mat_vec_n) < TESTS_TOL);
239
240
241 // cleanup
242 c_free(Ax);
243 c_free(ATy);
244 c_free(Px);
245 c_free(Ax_cum);
246 c_free(ATy_cum);
247 c_free(Px_cum);
248 clean_problem_lin_alg_sols_data(data);
249}
250
251void test_extract_upper_triangular() {
252 c_float *inf_norm_cols_test;
253 lin_alg_sols_data *data = generate_problem_lin_alg_sols_data();
254
255 // Extract upper triangular part
256 csc *Ptriu = csc_to_triu(data->test_mat_extr_triu_P);
257
258 mu_assert("Linear algebra tests: error in forming upper triangular matrix!",
259 is_eq_csc(data->test_mat_extr_triu_Pu, Ptriu, TESTS_TOL));
260
261 // Compute infinity norm over columns of the original matrix by using the
262 // upper triangular part only
263 inf_norm_cols_test = (c_float *)c_malloc(data->test_mat_extr_triu_n
264 * sizeof(c_float));
265 mat_inf_norm_cols_sym_triu(Ptriu, inf_norm_cols_test);
266 mu_assert(
267 "Linear algebra tests: error in forming upper triangular matrix, infinity norm over columns",
268 vec_norm_inf_diff(inf_norm_cols_test,
269 data->test_mat_extr_triu_P_inf_norm_cols,
270 data->test_mat_extr_triu_n) < TESTS_TOL);
271
272 // Cleanup
273 c_free(inf_norm_cols_test);
274 csc_spfree(Ptriu);
275 clean_problem_lin_alg_sols_data(data);
276}
277
278void test_quad_form_upper_triang() {
279 c_float quad_form_t;
280
281 lin_alg_sols_data *data = generate_problem_lin_alg_sols_data();
282
283 // Compute quadratic form
284 quad_form_t = quad_form(data->test_qpform_Pu, data->test_qpform_x);
285
286 mu_assert(
287 "Linear algebra tests: error in computing quadratic form using upper triangular matrix!",
288 (c_absval(quad_form_t - data->test_qpform_value) < TESTS_TOL));
289
290 // cleanup
291 clean_problem_lin_alg_sols_data(data);
292}