1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
|
#include <gtest/gtest.h>
#include "manif/manif.h"
#include "gtest_manif_utils.h"
#include "gtest_eigen_utils.h"
using namespace manif;
TEST(TEST_MISC, TEST_SKEW)
{
constexpr double s = 1.5;
EXPECT_EIGEN_NEAR((Eigen::Matrix2d() << 0., -s, s, 0.).finished(), skew(s));
}
TEST(TEST_MISC, TEST_SKEW3)
{
EXPECT_EIGEN_NEAR(
(Eigen::Matrix3d() <<
0., -3.7, +2.6,
+3.7, 0., -1.5,
-2.6, +1.5, 0.).finished(),
skew(Eigen::Vector3d(1.5, 2.6, 3.7))
);
}
TEST(TEST_MISC, TEST_SKEWd)
{
Eigen::VectorXd s(1, 1);
s << 1.5;
EXPECT_EIGEN_NEAR((Eigen::Matrix2d() << 0., -1.5, 1.5, 0.).finished(), skew(s));
s.resize(3, 1);
s << 1.5, 2.6, 3.7;
EXPECT_EIGEN_NEAR((Eigen::Matrix3d() <<
0., -3.7, +2.6,
+3.7, 0., -1.5,
-2.6, +1.5, 0.).finished(), skew(s));
s.resize(5, 1);
s << 1.5, 2.6, 3.7, 1., 1.;
EXPECT_THROW(skew(s), manif::runtime_error);
}
MANIF_RUN_ALL_TEST;
|
