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/Symbol.h>
21 
22 #include <CppUnitLite/TestHarness.h>
23 
24 #include <sstream>
25 
26 using namespace std;
27 using namespace gtsam;
28 
29 /* ************************************************************************* */
30 TEST(VectorValues, basics)
31 {
32  // Tests insert(), size(), dim(), exists(), vector()
33 
34  // insert
35  VectorValues actual;
36  actual.insert(0, (Vector(1) << 1).finished());
37  actual.insert(1, Vector2(2, 3));
38  actual.insert(5, Vector2(6, 7));
39  actual.insert(2, Vector2(4, 5));
40 
41  // Check dimensions
42  LONGS_EQUAL(4, actual.size());
43  LONGS_EQUAL(1, actual.dim(0));
44  LONGS_EQUAL(2, actual.dim(1));
45  LONGS_EQUAL(2, actual.dim(2));
46  LONGS_EQUAL(2, actual.dim(5));
47 
48  // Logic
49  EXPECT(actual.exists(0));
50  EXPECT(actual.exists(1));
51  EXPECT(actual.exists(2));
52  EXPECT(!actual.exists(3));
53  EXPECT(!actual.exists(4));
54  EXPECT(actual.exists(5));
55  EXPECT(!actual.exists(6));
56 
57  // Check values
58  EXPECT(assert_equal((Vector(1) << 1).finished(), actual[0]));
59  EXPECT(assert_equal(Vector2(2, 3), actual[1]));
60  EXPECT(assert_equal(Vector2(4, 5), actual[2]));
61  EXPECT(assert_equal(Vector2(6, 7), actual[5]));
62  KeyVector keys {0, 1, 2, 5};
63  EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7).finished(), actual.vector(keys)));
64 
65  // Check exceptions
66  CHECK_EXCEPTION(actual.insert(1, Vector()), invalid_argument);
67  CHECK_EXCEPTION(actual.dim(3), out_of_range);
68 }
69 
70 /* ************************************************************************* */
71 
72 static const VectorValues kExample = {{99, Vector2(2, 3)}};
73 
74 // Check insert
75 TEST(VectorValues, Insert) {
76  VectorValues actual;
77  EXPECT(assert_equal(kExample, actual.insert(kExample)));
78 }
79 
80 // Check update.
81 TEST(VectorValues, Update) {
82  VectorValues actual(kExample);
83  EXPECT(assert_equal(kExample, actual.update(kExample)));
84 }
85 
86 /* ************************************************************************* */
87 TEST(VectorValues, combine)
88 {
89  VectorValues expected;
90  expected.insert(0, (Vector(1) << 1).finished());
91  expected.insert(1, Vector2(2, 3));
92  expected.insert(5, Vector2(6, 7));
93  expected.insert(2, Vector2(4, 5));
94 
95  VectorValues first;
96  first.insert(0, (Vector(1) << 1).finished());
97  first.insert(1, Vector2(2, 3));
98 
99  VectorValues second;
100  second.insert(5, Vector2(6, 7));
101  second.insert(2, Vector2(4, 5));
102 
103  VectorValues actual(first, second);
104 
105  EXPECT(assert_equal(expected, actual));
106 }
107 
108 /* ************************************************************************* */
109 TEST(VectorValues, subvector)
110 {
111  VectorValues init;
112  init.insert(10, (Vector(1) << 1).finished());
113  init.insert(11, Vector2(2, 3));
114  init.insert(12, Vector2(4, 5));
115  init.insert(13, Vector2(6, 7));
116 
117  KeyVector keys {10, 12, 13};
118  Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7).finished();
119  EXPECT(assert_equal(expSubVector, init.vector(keys)));
120 }
121 
122 /* ************************************************************************* */
123 TEST(VectorValues, LinearAlgebra)
124 {
125  VectorValues test1;
126  test1.insert(0, (Vector(1) << 1).finished());
127  test1.insert(1, Vector2(2, 3));
128  test1.insert(5, Vector2(6, 7));
129  test1.insert(2, Vector2(4, 5));
130 
131  VectorValues test2;
132  test2.insert(0, (Vector(1) << 6).finished());
133  test2.insert(1, Vector2(1, 6));
134  test2.insert(5, Vector2(4, 3));
135  test2.insert(2, Vector2(1, 8));
136 
137  // Dot product
138  double dotExpected = test1.vector().dot(test2.vector());
139  double dotActual = test1.dot(test2);
140  DOUBLES_EQUAL(dotExpected, dotActual, 1e-10);
141 
142  // Norm
143  double normExpected = test1.vector().norm();
144  double normActual = test1.norm();
145  DOUBLES_EQUAL(normExpected, normActual, 1e-10);
146 
147  // Squared norm
148  double sqNormExpected = test1.vector().norm();
149  double sqNormActual = test1.norm();
150  DOUBLES_EQUAL(sqNormExpected, sqNormActual, 1e-10);
151 
152  // Addition
153  Vector sumExpected = test1.vector() + test2.vector();
154  VectorValues sumActual = test1 + test2;
155  EXPECT(sumActual.hasSameStructure(test1));
156  EXPECT(assert_equal(sumExpected, sumActual.vector()));
157  Vector sum2Expected = sumActual.vector() + test1.vector();
158  VectorValues sum2Actual = sumActual;
159  sum2Actual += test1;
160  EXPECT(assert_equal(sum2Expected, sum2Actual.vector()));
161 
162  // Add to empty
163  VectorValues sumActual3;
164  sumActual3.addInPlace_(test1);
165  sumActual3.addInPlace_(test2);
166  EXPECT(assert_equal(sumExpected, sumActual3.vector()));
167 
168  // Subtraction
169  Vector difExpected = test1.vector() - test2.vector();
170  VectorValues difActual = test1 - test2;
171  EXPECT(difActual.hasSameStructure(test1));
172  EXPECT(assert_equal(difExpected, difActual.vector()));
173 
174  // Scaling
175  Vector scalExpected = test1.vector() * 5.0;
176  VectorValues scalActual = test1;
177  scalActual *= 5.0;
178  EXPECT(assert_equal(scalExpected, scalActual.vector()));
179  VectorValues scal2Actual = 5.0 * test1;
180  EXPECT(assert_equal(scalExpected, scal2Actual.vector()));
181 }
182 
183 /* ************************************************************************* */
184 TEST(VectorValues, convert)
185 {
186  Vector x(7);
187  x << 1, 2, 3, 4, 5, 6, 7;
188 
189  VectorValues expected;
190  expected.insert(0, (Vector(1) << 1).finished());
191  expected.insert(1, Vector2(2, 3));
192  expected.insert(2, Vector2(4, 5));
193  expected.insert(5, Vector2(6, 7));
194 
195  std::map<Key,size_t> dims;
196  dims.insert(make_pair(0,1));
197  dims.insert(make_pair(1,2));
198  dims.insert(make_pair(2,2));
199  dims.insert(make_pair(5,2));
200  VectorValues actual(x,dims);
201  EXPECT(assert_equal(expected, actual));
202 
203  Scatter scatter;
204  scatter.emplace_back(0,1);
205  scatter.emplace_back(1,2);
206  scatter.emplace_back(2,2);
207  scatter.emplace_back(5,2);
208  VectorValues actual2(x,scatter);
209  EXPECT(assert_equal(expected, actual2));
210 
211  // Test other direction, note vector() is not guaranteed to give right result
212  KeyVector keys {0, 1, 2, 5};
213  EXPECT(assert_equal(x, actual.vector(keys)));
214 
215  // Test version with dims argument
216  EXPECT(assert_equal(x, actual.vector(dims)));
217 }
218 
219 /* ************************************************************************* */
220 TEST(VectorValues, vector_sub)
221 {
222  VectorValues vv;
223  vv.insert(0, (Vector(1) << 1).finished());
224  vv.insert(1, Vector2(2, 3));
225  vv.insert(2, Vector2(4, 5));
226  vv.insert(5, Vector2(6, 7));
227  vv.insert(7, Vector2(8, 9));
228 
229  std::map<Key,size_t> dims;
230  dims.insert(make_pair(0,1));
231  dims.insert(make_pair(5,2));
232 
233  Vector expected(3);
234  expected << 1, 6, 7;
235 
236  // Test FastVector version
237  KeyVector keys {0, 5};
238  EXPECT(assert_equal(expected, vv.vector(keys)));
239 
240  // Test version with dims argument
241  EXPECT(assert_equal(expected, vv.vector(dims)));
242 }
243 
244 /* ************************************************************************* */
245 TEST(VectorValues, print)
246 {
247  VectorValues vv;
248  vv.insert(0, (Vector(1) << 1).finished());
249  vv.insert(1, Vector2(2, 3));
250  vv.insert(2, Vector2(4, 5));
251  vv.insert(5, Vector2(6, 7));
252  vv.insert(7, Vector2(8, 9));
253 
254  string expected =
255  " 0: 1\n 1: 2 3\n 2: 4 5\n 5: 6 7\n 7: 8 9\n";
256  stringstream actual;
257  actual << vv;
258  EXPECT(expected == actual.str());
259 }
260 
261 /* ************************************************************************* */
262 // Check html representation.
263 TEST(VectorValues, html) {
264  VectorValues vv;
265  using symbol_shorthand::X;
266  vv.insert(X(1), Vector2(2, 3.1));
267  vv.insert(X(2), Vector2(4, 5.2));
268  vv.insert(X(5), Vector2(6, 7.3));
269  vv.insert(X(7), Vector2(8, 9.4));
270  string expected =
271  "<div>\n"
272  "<table class='VectorValues'>\n"
273  " <thead>\n"
274  " <tr><th>Variable</th><th>value</th></tr>\n"
275  " </thead>\n"
276  " <tbody>\n"
277  " <tr><th>x1</th><td> 2 3.1</td></tr>\n"
278  " <tr><th>x2</th><td> 4 5.2</td></tr>\n"
279  " <tr><th>x5</th><td> 6 7.3</td></tr>\n"
280  " <tr><th>x7</th><td> 8 9.4</td></tr>\n"
281  " </tbody>\n"
282  "</table>\n"
283  "</div>";
284  string actual = vv.html();
285  EXPECT(actual == expected);
286 }
287 
288 /* ************************************************************************* */
289 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
290 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
CHECK_EXCEPTION
#define CHECK_EXCEPTION(condition, exception_name)
Definition: Test.h:118
main
int main()
Definition: testVectorValues.cpp:289
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
TestHarness.h
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:90
gtsam::VectorValues::html
std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Output as a html table.
Definition: VectorValues.cpp:390
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
gtsam::VectorValues::hasSameStructure
bool hasSameStructure(const VectorValues other) const
Definition: VectorValues.cpp:228
kExample
static const VectorValues kExample
Definition: testVectorValues.cpp:72
gtsam::VectorValues::dot
double dot(const VectorValues &v) const
Definition: VectorValues.cpp:237
X
#define X
Definition: icosphere.cpp:20
vv
static const VectorValues vv
Definition: testHybridGaussianConditional.cpp:44
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::VectorValues::addInPlace_
VectorValues & addInPlace_(const VectorValues &c)
Definition: VectorValues.cpp:318
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::VectorValues::update
VectorValues & update(const VectorValues &values)
Definition: VectorValues.cpp:100
DOUBLES_EQUAL
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::Scatter
Definition: Scatter.h:49
Symbol.h
VectorValues.h
Factor Graph Values.
TEST
TEST(VectorValues, basics)
Definition: testVectorValues.cpp:30
init
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:2006
gtsam::VectorValues::dim
size_t dim(Key j) const
Definition: VectorValues.h:133
TestResult
Definition: TestResult.h:26
gtsam::VectorValues::norm
double norm() const
Definition: VectorValues.cpp:255
gtsam
traits
Definition: SFMdata.h:40
std
Definition: BFloat16.h:88
gtsam::VectorValues::exists
bool exists(Key j) const
Definition: VectorValues.h:136
gtsam::VectorValues::size
size_t size() const
Definition: VectorValues.h:130
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::html
string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of html.
Definition: DiscreteValues.cpp:135
test2
void test2(OptionalJacobian<-1,-1 > H={})
Definition: testOptionalJacobian.cpp:123
init
Definition: TutorialInplaceLU.cpp:2
gtsam::convert
static BinaryMeasurement< Rot3 > convert(const BetweenFactor< Pose3 >::shared_ptr &f)
Definition: ShonanAveraging.cpp:993
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
LONGS_EQUAL
#define LONGS_EQUAL(expected, actual)
Definition: Test.h:134
gtsam::VectorValues::vector
Vector vector() const
Definition: VectorValues.cpp:175

gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:08:56