gtsam
nonlinear
tests
testUtilities.cpp
Go to the documentation of this file.
1
/* ----------------------------------------------------------------------------
2
3
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
4
* Atlanta, Georgia 30332-0415
5
* All Rights Reserved
6
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8
* See LICENSE for the license information
9
10
* -------------------------------------------------------------------------- */
11
12
/*
13
* @file testUtilities.cpp
14
* @date Aug 19, 2021
15
* @author Varun Agrawal
16
* @brief Tests for the utilities.
17
*/
18
19
#include <
CppUnitLite/TestHarness.h
>
20
#include <
gtsam/base/Testable.h
>
21
#include <
gtsam/geometry/Point2.h
>
22
#include <
gtsam/geometry/Pose3.h
>
23
#include <
gtsam/inference/Symbol.h
>
24
#include <
gtsam/nonlinear/utilities.h
>
25
26
using namespace
gtsam
;
27
using
gtsam::symbol_shorthand::L
;
28
using
gtsam::symbol_shorthand::R
;
29
using
gtsam::symbol_shorthand::X
;
30
31
/* ************************************************************************* */
32
TEST
(Utilities, ExtractPoint2) {
33
Point2
p0
(0, 0),
p1
(1, 0);
34
Values
values
;
35
values
.
insert
<
Point2
>(
L
(0),
p0
);
36
values
.
insert
<
Point2
>(
L
(1),
p1
);
37
values
.
insert
<
Rot3
>(
R
(0),
Rot3
());
38
values
.
insert
<
Pose3
>(
X
(0),
Pose3
());
39
40
Matrix
all_points =
utilities::extractPoint2
(
values
);
41
EXPECT_LONGS_EQUAL
(2, all_points.rows());
42
}
43
44
/* ************************************************************************* */
45
TEST
(Utilities, ExtractPoint3) {
46
Point3
p0
(0, 0, 0),
p1
(1, 0, 0);
47
Values
values
;
48
values
.
insert
<
Point3
>(
L
(0),
p0
);
49
values
.
insert
<
Point3
>(
L
(1),
p1
);
50
values
.
insert
<
Rot3
>(
R
(0),
Rot3
());
51
values
.
insert
<
Pose3
>(
X
(0),
Pose3
());
52
53
Matrix
all_points =
utilities::extractPoint3
(
values
);
54
EXPECT_LONGS_EQUAL
(2, all_points.rows());
55
}
56
57
/* ************************************************************************* */
58
TEST
(Utilities, ExtractVector) {
59
// Test normal case with 3 vectors and 1 non-vector (ignore non-vector)
60
auto
values
=
Values
();
61
values
.
insert
(
X
(0), (
Vector
(4) << 1, 2, 3, 4).finished());
62
values
.
insert
(
X
(2), (
Vector
(4) << 13, 14, 15, 16).finished());
63
values
.
insert
(
X
(1), (
Vector
(4) << 6, 7, 8, 9).finished());
64
values
.
insert
(
X
(3),
Pose3
());
65
auto
actual =
utilities::extractVectors
(
values
,
'x'
);
66
auto
expected
=
67
(
Matrix
(3, 4) << 1, 2, 3, 4, 6, 7, 8, 9, 13, 14, 15, 16).finished();
68
EXPECT
(
assert_equal
(
expected
, actual));
69
70
// Check that mis-sized vectors fail
71
values
.
insert
(
X
(4), (
Vector
(2) << 1, 2).finished());
72
THROWS_EXCEPTION
(
utilities::extractVectors
(
values
,
'x'
));
73
values
.
update
(
X
(4), (
Vector
(6) << 1, 2, 3, 4, 5, 6).finished());
74
THROWS_EXCEPTION
(
utilities::extractVectors
(
values
,
'x'
));
75
}
76
77
/* ************************************************************************* */
78
int
main
() {
79
srand(
time
(
nullptr
));
80
TestResult
tr;
81
return
TestRegistry::runAllTests
(tr);
82
}
83
/* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition:
TestRegistry.cpp:27
utilities.h
Contains generic global functions designed particularly for the matlab interface.
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition:
Test.h:154
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition:
Test.h:150
TestHarness.h
gtsam::HybridValues::update
HybridValues & update(const VectorValues &values)
Definition:
HybridValues.cpp:135
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition:
base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition:
testHybridBayesNet.cpp:245
X
#define X
Definition:
icosphere.cpp:20
gtsam::Vector
Eigen::VectorXd Vector
Definition:
Vector.h:38
Point2.h
2D Point
gtsam::symbol_shorthand::X
Key X(std::uint64_t j)
Definition:
inference/Symbol.h:171
gtsam::utilities::extractPoint3
Matrix extractPoint3(const Values &values)
Extract all Point3 values into a single matrix [x y z].
Definition:
nonlinear/utilities.h:112
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition:
Rot3.h:58
gtsam::Pose3
Definition:
Pose3.h:37
cholesky::expected
Matrix expected
Definition:
testMatrix.cpp:971
gtsam::symbol_shorthand::R
Key R(std::uint64_t j)
Definition:
inference/Symbol.h:165
L
MatrixXd L
Definition:
LLT_example.cpp:6
Symbol.h
gtsam::symbol_shorthand::L
Key L(std::uint64_t j)
Definition:
inference/Symbol.h:159
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition:
HybridValues.cpp:85
gtsam::Point2
Vector2 Point2
Definition:
Point2.h:32
time
#define time
Definition:
timeAdaptAutoDiff.cpp:31
main
int main()
Definition:
testUtilities.cpp:78
p1
Vector3f p1
Definition:
MatrixBase_all.cpp:2
gtsam::utilities::extractVectors
Matrix extractVectors(const Values &values, char c)
Definition:
nonlinear/utilities.h:180
TestResult
Definition:
TestResult.h:26
Values
std::vector< float > Values
Definition:
sparse_setter.cpp:45
gtsam
traits
Definition:
SFMdata.h:40
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition:
testSmartFactorBase.cpp:38
gtsam::Values
Definition:
Values.h:65
p0
Vector3f p0
Definition:
MatrixBase_all.cpp:2
gtsam::utilities::extractPoint2
Matrix extractPoint2(const Values &values)
Extract all Point2 values into a single matrix [x y].
Definition:
nonlinear/utilities.h:92
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition:
Matrix.cpp:40
gtsam::Point3
Vector3 Point3
Definition:
Point3.h:38
THROWS_EXCEPTION
#define THROWS_EXCEPTION(condition)
Definition:
Test.h:112
R
Rot2 R(Rot2::fromAngle(0.1))
Pose3.h
3D Pose
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:03