testVectorValues.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 
18 #include <gtsam/base/Testable.h>
19 #include <gtsam/linear/VectorValues.h>
20 #include <gtsam/inference/LabeledSymbol.h>
21 
22 #include <CppUnitLite/TestHarness.h>
23 
24 #include <boost/assign/std/vector.hpp>
25 #include <boost/assign/list_of.hpp>
26 #include <boost/range/adaptor/map.hpp>
27 
28 #include <sstream>
29 
30 using namespace std;
31 using namespace boost::assign;
32 using boost::adaptors::map_keys;
33 using namespace gtsam;
34 
35 /* ************************************************************************* */
36 TEST(VectorValues, basics)
37 {
38  // Tests insert(), size(), dim(), exists(), vector()
39 
40  // insert
41  VectorValues actual;
42  actual.insert(0, (Vector(1) << 1).finished());
43  actual.insert(1, Vector2(2, 3));
44  actual.insert(5, Vector2(6, 7));
45  actual.insert(2, Vector2(4, 5));
46 
47  // Check dimensions
48  LONGS_EQUAL(4, actual.size());
49  LONGS_EQUAL(1, actual.dim(0));
50  LONGS_EQUAL(2, actual.dim(1));
51  LONGS_EQUAL(2, actual.dim(2));
52  LONGS_EQUAL(2, actual.dim(5));
53 
54  // Logic
55  EXPECT(actual.exists(0));
56  EXPECT(actual.exists(1));
57  EXPECT(actual.exists(2));
58  EXPECT(!actual.exists(3));
59  EXPECT(!actual.exists(4));
60  EXPECT(actual.exists(5));
61  EXPECT(!actual.exists(6));
62 
63  // Check values
64  EXPECT(assert_equal((Vector(1) << 1).finished(), actual[0]));
65  EXPECT(assert_equal(Vector2(2, 3), actual[1]));
66  EXPECT(assert_equal(Vector2(4, 5), actual[2]));
67  EXPECT(assert_equal(Vector2(6, 7), actual[5]));
68  KeyVector keys {0, 1, 2, 5};
69  EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7).finished(), actual.vector(keys)));
70 
71  // Check exceptions
72  CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
73  CHECK_EXCEPTION(actual.dim(3), out_of_range);
74 }
75 
76 /* ************************************************************************* */
77 TEST(VectorValues, combine)
78 {
79  VectorValues expected;
80  expected.insert(0, (Vector(1) << 1).finished());
81  expected.insert(1, Vector2(2, 3));
82  expected.insert(5, Vector2(6, 7));
83  expected.insert(2, Vector2(4, 5));
84 
85  VectorValues first;
86  first.insert(0, (Vector(1) << 1).finished());
87  first.insert(1, Vector2(2, 3));
88 
89  VectorValues second;
90  second.insert(5, Vector2(6, 7));
91  second.insert(2, Vector2(4, 5));
92 
93  VectorValues actual(first, second);
94 
95  EXPECT(assert_equal(expected, actual));
96 }
97 
98 /* ************************************************************************* */
99 TEST(VectorValues, subvector)
100 {
101  VectorValues init;
102  init.insert(10, (Vector(1) << 1).finished());
103  init.insert(11, Vector2(2, 3));
104  init.insert(12, Vector2(4, 5));
105  init.insert(13, Vector2(6, 7));
106 
107  KeyVector keys {10, 12, 13};
108  Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7).finished();
109  EXPECT(assert_equal(expSubVector, init.vector(keys)));
110 }
111 
112 /* ************************************************************************* */
113 TEST(VectorValues, LinearAlgebra)
114 {
115  VectorValues test1;
116  test1.insert(0, (Vector(1) << 1).finished());
117  test1.insert(1, Vector2(2, 3));
118  test1.insert(5, Vector2(6, 7));
119  test1.insert(2, Vector2(4, 5));
120 
121  VectorValues test2;
122  test2.insert(0, (Vector(1) << 6).finished());
123  test2.insert(1, Vector2(1, 6));
124  test2.insert(5, Vector2(4, 3));
125  test2.insert(2, Vector2(1, 8));
126 
127  // Dot product
128  double dotExpected = test1.vector().dot(test2.vector());
129  double dotActual = test1.dot(test2);
130  DOUBLES_EQUAL(dotExpected, dotActual, 1e-10);
131 
132  // Norm
133  double normExpected = test1.vector().norm();
134  double normActual = test1.norm();
135  DOUBLES_EQUAL(normExpected, normActual, 1e-10);
136 
137  // Squared norm
138  double sqNormExpected = test1.vector().norm();
139  double sqNormActual = test1.norm();
140  DOUBLES_EQUAL(sqNormExpected, sqNormActual, 1e-10);
141 
142  // Addition
143  Vector sumExpected = test1.vector() + test2.vector();
144  VectorValues sumActual = test1 + test2;
145  EXPECT(sumActual.hasSameStructure(test1));
146  EXPECT(assert_equal(sumExpected, sumActual.vector()));
147  Vector sum2Expected = sumActual.vector() + test1.vector();
148  VectorValues sum2Actual = sumActual;
149  sum2Actual += test1;
150  EXPECT(assert_equal(sum2Expected, sum2Actual.vector()));
151 
152  // Add to empty
153  VectorValues sumActual3;
154  sumActual3.addInPlace_(test1);
155  sumActual3.addInPlace_(test2);
156  EXPECT(assert_equal(sumExpected, sumActual3.vector()));
157 
158  // Subtraction
159  Vector difExpected = test1.vector() - test2.vector();
160  VectorValues difActual = test1 - test2;
161  EXPECT(difActual.hasSameStructure(test1));
162  EXPECT(assert_equal(difExpected, difActual.vector()));
163 
164  // Scaling
165  Vector scalExpected = test1.vector() * 5.0;
166  VectorValues scalActual = test1;
167  scalActual *= 5.0;
168  EXPECT(assert_equal(scalExpected, scalActual.vector()));
169  VectorValues scal2Actual = 5.0 * test1;
170  EXPECT(assert_equal(scalExpected, scal2Actual.vector()));
171 }
172 
173 /* ************************************************************************* */
174 TEST(VectorValues, convert)
175 {
176  Vector x(7);
177  x << 1, 2, 3, 4, 5, 6, 7;
178 
179  VectorValues expected;
180  expected.insert(0, (Vector(1) << 1).finished());
181  expected.insert(1, Vector2(2, 3));
182  expected.insert(2, Vector2(4, 5));
183  expected.insert(5, Vector2(6, 7));
184 
185  std::map<Key,size_t> dims;
186  dims.insert(make_pair(0,1));
187  dims.insert(make_pair(1,2));
188  dims.insert(make_pair(2,2));
189  dims.insert(make_pair(5,2));
190  VectorValues actual(x,dims);
191  EXPECT(assert_equal(expected, actual));
192 
193  Scatter scatter;
194  scatter.emplace_back(0,1);
195  scatter.emplace_back(1,2);
196  scatter.emplace_back(2,2);
197  scatter.emplace_back(5,2);
198  VectorValues actual2(x,scatter);
199  EXPECT(assert_equal(expected, actual2));
200 
201  // Test other direction, note vector() is not guaranteed to give right result
202  KeyVector keys {0, 1, 2, 5};
203  EXPECT(assert_equal(x, actual.vector(keys)));
204 
205  // Test version with dims argument
206  EXPECT(assert_equal(x, actual.vector(dims)));
207 }
208 
209 /* ************************************************************************* */
210 TEST(VectorValues, vector_sub)
211 {
212  VectorValues vv;
213  vv.insert(0, (Vector(1) << 1).finished());
214  vv.insert(1, Vector2(2, 3));
215  vv.insert(2, Vector2(4, 5));
216  vv.insert(5, Vector2(6, 7));
217  vv.insert(7, Vector2(8, 9));
218 
219  std::map<Key,size_t> dims;
220  dims.insert(make_pair(0,1));
221  dims.insert(make_pair(5,2));
222 
223  Vector expected(3);
224  expected << 1, 6, 7;
225 
226  // Test FastVector version
227  KeyVector keys {0, 5};
228  EXPECT(assert_equal(expected, vv.vector(keys)));
229 
230  // Test version with dims argument
231  EXPECT(assert_equal(expected, vv.vector(dims)));
232 }
233 
234 /* ************************************************************************* */
235 TEST(VectorValues, print)
236 {
237  VectorValues vv;
238  vv.insert(0, (Vector(1) << 1).finished());
239  vv.insert(1, Vector2(2, 3));
240  vv.insert(2, Vector2(4, 5));
241  vv.insert(5, Vector2(6, 7));
242  vv.insert(7, Vector2(8, 9));
243 
244  string expected =
245  " 0: 1\n 1: 2 3\n 2: 4 5\n 5: 6 7\n 7: 8 9\n";
246  stringstream actual;
247  actual << vv;
248  EXPECT(expected == actual.str());
249 }
250 
251 /* ************************************************************************* */
252 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
253 /* ************************************************************************* */
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
main
int main()
Definition: testVectorValues.cpp:252
gtsam::VectorValues::norm
double norm() const
Definition: VectorValues.cpp:247
Testable.h
Concept check for values that can be used in unit tests.
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
gtsam::VectorValues::exists
bool exists(Key j) const
Definition: VectorValues.h:131
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:974
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:142
gtsam::VectorValues::addInPlace_
VectorValues & addInPlace_(const VectorValues &c)
Definition: VectorValues.cpp:310
gtsam::VectorValues::size
size_t size() const
Definition: VectorValues.h:125
std
Definition: Half.h:150
CHECK_EXCEPTION
#define CHECK_EXCEPTION(condition, exception_name)
Definition: Test.h:119
VectorValues.h
Factor Graph Values.
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
first
constexpr int first(int i)
Implementation details for constexpr functions.
Definition: wrap/pybind11/include/pybind11/detail/common.h:570
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::VectorValues::hasSameStructure
bool hasSameStructure(const VectorValues other) const
Definition: VectorValues.cpp:222
convert
bool convert(const int &y)
Definition: testDecisionTree.cpp:183
EXPECT
#define EXPECT(condition)
Definition: Test.h:151
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::VectorValues::dot
double dot(const VectorValues &v) const
Definition: VectorValues.cpp:229
TEST
TEST(VectorValues, basics)
Definition: testVectorValues.cpp:36
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:135
gtsam
traits
Definition: chartTesting.h:28
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:42
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
init
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:1460
gtsam::VectorValues::dim
size_t dim(Key j) const
Definition: VectorValues.h:128
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:45
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:90
gtsam::VectorValues::vector
Vector vector() const
Definition: VectorValues.cpp:173
test2
void test2(OptionalJacobian<-1,-1 > H=boost::none)
Definition: testOptionalJacobian.cpp:107

gtsam
Author(s):
autogenerated on Sat May 8 2021 02:50:25