Skip to content
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.

Commit 16331a1

Browse files
author
Luca Olivieri
committedFeb 10, 2020
Preliminary results on delta
1 parent 0d05d08 commit 16331a1

File tree

8 files changed

+114
-43
lines changed

8 files changed

+114
-43
lines changed
 

‎.vscode/launch.json

+1-1
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
"name": "C++ Debug",
99
"type": "cppdbg",
1010
"request": "launch",
11-
"program": "${fileDirname}/../build/debugVSC/${fileBasenameNoExtension}",
11+
"program": "${fileDirname}/../build/${fileBasenameNoExtension}",
1212
"args": [],
1313
"stopAtEntry": false,
1414
"cwd": "${workspaceFolder}",

‎.vscode/settings.json

+3-1
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,8 @@
66
"ref_ptr": "cpp",
77
"matrix": "cpp",
88
"core": "cpp",
9-
"cmath": "cpp"
9+
"cmath": "cpp",
10+
"chrono": "cpp",
11+
"complex": "cpp"
1012
},
1113
}

‎.vscode/tasks.json

+1-1
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111
"-I/opt/openrobots/include/eigen3",
1212
"-I/${fileDirname}/../include",
1313
"-o",
14-
"${fileDirname}/../build/debugVSC/${fileBasenameNoExtension}"
14+
"${fileDirname}/../build/${fileBasenameNoExtension}"
1515
],
1616
"options": {
1717
"cwd": "/usr/bin"

‎CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@ ENDIF(WIN32)
3939
# ----------------------------------------------------
4040

4141
ADD_DEFINITIONS(-DEIGEN_RUNTIME_NO_MALLOC)
42+
ADD_DEFINITIONS(-DPROFILERYES)
4243

4344
# uncomment these lines to use LAPACK
4445
#SEARCH_FOR_LAPACK()

‎include/MatrixExponential.hpp

+19-30
Original file line numberDiff line numberDiff line change
@@ -13,12 +13,12 @@
1313
#define EIGEN_MALLOC_NOT_ALLOWED
1414
#endif
1515

16+
#include "unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h"
17+
#include "utils/stop-watch.h"
1618
#include <Eigen/Core>
1719
#include <Eigen/LU>
1820
#include <iostream>
1921
#include <stdio.h>
20-
#include "unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h"
21-
#include "utils/stop-watch.h"
2222

2323
#ifdef __cplusplus
2424
extern "C" {
@@ -35,7 +35,6 @@ template <typename T, int N>
3535
class MatrixExponential {
3636

3737
private:
38-
3938
int size;
4039

4140
// Typedefs to make code more readable
@@ -55,7 +54,7 @@ class MatrixExponential {
5554
int squarings;
5655

5756
MatrixType prevEA, prevA, deltaA; // Used in delta update
58-
bool delta;
57+
bool delta, deltaUsed;
5958

6059
public:
6160
MatrixExponential();
@@ -66,9 +65,11 @@ class MatrixExponential {
6665

6766
int get_squarings() const { return squarings; }
6867

69-
void setDelta(bool delta) { this->delta = delta; }
68+
void useDelta(bool delta) { this->delta = delta; }
7069
bool getDelta() { return delta; }
7170

71+
bool wasDeltaUsed() { return deltaUsed; }
72+
7273
/** Compute the exponential of the given matrix arg and writes it in result.
7374
*/
7475
void compute(RefMatrix A, RefOutMatrix out);
@@ -132,13 +133,11 @@ class MatrixExponential {
132133
template <typename T, int N>
133134
MatrixExponential<T, N>::MatrixExponential()
134135
{
135-
START_PROFILER("MatrixExponential::constructor");
136136
if (N == Dynamic) {
137137
init(2); // Init to dym 2 for no particular reason
138138
} else {
139139
init(N);
140140
}
141-
STOP_PROFILER("MatrixExponential::constructor");
142141
}
143142

144143
template <typename T, int N>
@@ -178,27 +177,28 @@ void MatrixExponential<T, N>::resize(int n)
178177
}
179178

180179
template <typename T, int N>
181-
void MatrixExponential<T, N>::resetDelta() {
180+
void MatrixExponential<T, N>::resetDelta()
181+
{
182182
// Necessary to trigger full computation at the first call
183-
prevA = MatrixType::Zero(size, size);
183+
prevA = MatrixType::Zero(size, size);
184184
prevEA = MatrixType::Identity(size, size);
185185
}
186186

187187
template <typename T, int N>
188188
void MatrixExponential<T, N>::compute(RefMatrix A, RefOutMatrix out)
189189
{
190-
bool deltaUsed = false;
190+
deltaUsed = false;
191191
if (delta) {
192-
deltaA = A - prevEA;
192+
deltaA = A - prevA;
193193
const double l1normA = A.cwiseAbs().colwise().sum().maxCoeff();
194194
const double l1normDeltaA = deltaA.cwiseAbs().colwise().sum().maxCoeff();
195195
// Speedup only if the difference in number of squaring is greater than 2
196196
deltaUsed = determineSquarings(l1normA) - determineSquarings(l1normDeltaA) > 1;
197-
}
198-
199-
if(deltaUsed)
200-
computeUV(deltaA);
201-
else
197+
}
198+
199+
if (deltaUsed)
200+
computeUV(deltaA);
201+
else
202202
computeUV(A); // Pade approximant is (U+V) / (-U+V)
203203
numer = U + V;
204204
denom = -U + V;
@@ -213,31 +213,24 @@ void MatrixExponential<T, N>::compute(RefMatrix A, RefOutMatrix out)
213213

214214
if (deltaUsed) { // Saving result for next delta computation
215215
out = prevEA * tmp;
216-
prevA = A;
217-
prevEA = tmp;
218216
} else {
219217
out = tmp;
220218
}
219+
220+
prevA = A;
221+
prevEA = out;
221222
}
222223

223224
template <typename T, int N>
224225
void MatrixExponential<T, N>::computeExpTimesVector(RefMatrix A, RefVector v, RefOutVector out, int vec_squarings)
225226
{
226-
START_PROFILER("MatrixExponential::computeExpTimesVector");
227-
START_PROFILER("MatrixExponential:computeExpTimesVector:computeUV");
228227
computeUV(A); // Pade approximant is (U+V) / (-U+V)
229-
STOP_PROFILER("MatrixExponential:computeExpTimesVector:computeUV");
230228
numer = U + V;
231229
denom = -U + V;
232-
START_PROFILER("MatrixExponential:computeExpTimesVector:computeLUdecomposition");
233230
ppLU.compute(denom);
234-
STOP_PROFILER("MatrixExponential:computeExpTimesVector:computeLUdecomposition");
235-
START_PROFILER("MatrixExponential:computeExpTimesVector:LUsolve");
236231
tmp = ppLU.solve(numer);
237-
STOP_PROFILER("MatrixExponential:computeExpTimesVector:LUsolve");
238232

239233
// undo scaling by repeated squaring
240-
START_PROFILER("MatrixExponential:computeExpTimesVector:squaringVector");
241234
/*
242235
* If the matrix size is n and the number of squaring is s, applying
243236
* matrix-vector multiplications is only convenient if
@@ -273,8 +266,6 @@ void MatrixExponential<T, N>::computeExpTimesVector(RefMatrix A, RefVector v, Re
273266
out.noalias() = tmp * v_tmp;
274267
v_tmp = out;
275268
}
276-
STOP_PROFILER("MatrixExponential:computeExpTimesVector:squaringVector");
277-
STOP_PROFILER("MatrixExponential::computeExpTimesVector");
278269
}
279270

280271
template <typename T, int N>
@@ -296,9 +287,7 @@ void MatrixExponential<T, N>::computeUV(const RefMatrix& A)
296287
if (l1norm > 2.097847961257068e+000) {
297288
squarings = determineSquarings(l1norm);
298289
A_scaled = A.unaryExpr(Eigen::internal::MatrixExponentialScalingOp<double>(squarings));
299-
START_PROFILER("MatrixExponential::matrix_exp_pade13");
300290
matrix_exp_pade13(A_scaled);
301-
STOP_PROFILER("MatrixExponential::matrix_exp_pade13");
302291
} else if (l1norm < 1.495585217958292e-002) {
303292
matrix_exp_pade3(A);
304293
} else if (l1norm < 2.539398330063230e-001) {

‎include/utils/stop-watch.h

+5-2
Original file line numberDiff line numberDiff line change
@@ -35,10 +35,13 @@ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
3535
#pragma GCC visibility push(default)
3636
#endif
3737

38+
#ifndef PROFILERYES
3839
#define START_PROFILER(name)
3940
#define STOP_PROFILER(name)
40-
//#define START_PROFILER(name) getProfiler().start(name)
41-
//#define STOP_PROFILER(name) getProfiler().stop(name)
41+
#else
42+
#define START_PROFILER(name) getProfiler().start(name)
43+
#define STOP_PROFILER(name) getProfiler().stop(name)
44+
#endif
4245

4346
#define STOP_WATCH_MAX_NAME_LENGTH 60
4447
#define STOP_WATCH_TIME_WIDTH 10

‎tests/CMakeLists.txt

+8-7
Original file line numberDiff line numberDiff line change
@@ -24,12 +24,13 @@ ENDMACRO(ADD_TESTCASE)
2424
# --- RULES -------------------------------------------------------------------
2525
#ADD_TESTCASE(test_dense_small "f2c")
2626
#ADD_TESTCASE(test_dense_large "f2c")
27-
ADD_TESTCASE(test_small "eigen3")
28-
ADD_TESTCASE(testIntegral "eigen3")
29-
ADD_TESTCASE(testMatrixExpo "eigen3")
30-
ADD_TESTCASE(testDyn "eigen3")
31-
ADD_TESTCASE(testNoMalloc "eigen3")
32-
ADD_TESTCASE(testSoundness "eigen3")
33-
ADD_TESTCASE(testTSV "eigen3")
27+
#ADD_TESTCASE(test_small "eigen3")
28+
#ADD_TESTCASE(testIntegral "eigen3")
29+
#ADD_TESTCASE(testMatrixExpo "eigen3")
30+
#ADD_TESTCASE(testDyn "eigen3")
31+
#ADD_TESTCASE(testNoMalloc "eigen3")
32+
#ADD_TESTCASE(testSoundness "eigen3")
33+
#ADD_TESTCASE(testTSV "eigen3")
34+
ADD_TESTCASE(testDelta "eigen3")
3435

3536

‎tests/testDelta.cpp

+76-1
Original file line numberDiff line numberDiff line change
@@ -3,14 +3,89 @@
33

44
#include "LDSUtility.hpp"
55

6+
#include "utils/stop-watch.h"
7+
68
using namespace std;
79
using namespace expokit;
810

911
/*
1012
Testing delta optimization
13+
- General correctness - test against official implementation
14+
- Speed against standard and official methods
1115
*/
1216

17+
#define MANY 100
18+
19+
#define N 4
20+
#define M N * 3 * 2
21+
1322
int main()
1423
{
15-
24+
cout << "Start test delta update" << endl;
25+
26+
Matrix<double, M, M> res0, res1, res2;
27+
int m2 = int(M / 2);
28+
double stiffness = 1e5;
29+
double damping = 1e2;
30+
MatrixXd U = MatrixXd::Random(m2, m2);
31+
MatrixXd Upsilon = U * U.transpose();
32+
MatrixXd K = MatrixXd::Identity(m2, m2) * stiffness;
33+
MatrixXd B = MatrixXd::Identity(m2, m2) * damping;
34+
MatrixXd A = MatrixXd::Zero(M, M);
35+
A.topRightCorner(m2, m2) = MatrixXd::Identity(m2, m2);
36+
A.bottomLeftCorner(m2, m2) = -Upsilon * K;
37+
A.bottomRightCorner(m2, m2) = -Upsilon * B;
38+
39+
array<MatrixXd, MANY> matrices{};
40+
41+
MatrixExponential<double, M> deltaOn;
42+
deltaOn.useDelta(true);
43+
44+
MatrixExponential<double, M> deltaOff;
45+
46+
// Checking correctness
47+
srand((unsigned int)time(NULL));
48+
for (int i = 0; i < MANY; ++i) {
49+
// Slightly changing matrix
50+
double smallChange = (rand() % 100) / 100.0;
51+
int index = rand() % 9;
52+
int sign = rand();
53+
if (sign % 2)
54+
A(index) += smallChange;
55+
else
56+
A(index) -= smallChange;
57+
58+
// Saving matrice for later
59+
matrices[i] = A;
60+
61+
// Computing using all three methods
62+
START_PROFILER("testDelta::official");
63+
res0 = A.exp();
64+
STOP_PROFILER("testDelta::official");
65+
66+
START_PROFILER("testDelta::deltaOff");
67+
deltaOff.compute(A, res1);
68+
STOP_PROFILER("testDelta::deltaOff");
69+
70+
START_PROFILER("testDelta::deltaOn");
71+
deltaOn.compute(A, res2);
72+
STOP_PROFILER("testDelta::deltaOn");
73+
74+
if ((res0 - res1).eval().cwiseAbs().sum() > 0) {
75+
cout << res0 - res1 << endl
76+
<< i << endl;
77+
return -1;
78+
}
79+
80+
// if ((res0 - res2).eval().cwiseAbs().sum() > 100) {
81+
// cout << res0 - res2 << endl
82+
cout << (res0 - res2).eval().cwiseAbs().sum() << '\t'
83+
<< (res0 - res2).eval().maxCoeff() << '\t'
84+
<< deltaOn.wasDeltaUsed() << '\t'
85+
<< i << endl;
86+
// return -2;
87+
// }
88+
}
89+
90+
getProfiler().report_all(3);
1691
}

0 commit comments

Comments
 (0)
Failed to load comments.