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 
20 #include <gtsam/base/Testable.h>
21 #include <gtsam/geometry/Point2.h>
22 #include <gtsam/geometry/Pose3.h>
23 #include <gtsam/inference/Symbol.h>
25 
26 using namespace gtsam;
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 
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 
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());
73  values.update(X(4), (Vector(6) << 1, 2, 3, 4, 5, 6).finished());
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 Sat Nov 16 2024 04:08:55