testDataset.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 
19 #include <gtsam/slam/dataset.h>
20 #include <gtsam/slam/BetweenFactor.h>
21 #include <gtsam/geometry/Pose2.h>
22 #include <gtsam/inference/Symbol.h>
23 #include <gtsam/base/TestableAssertions.h>
24 
25 #include <CppUnitLite/TestHarness.h>
26 
27 #include <iostream>
28 #include <sstream>
29 
30 using namespace gtsam::symbol_shorthand;
31 using namespace std;
32 using namespace gtsam;
33 
34 /* ************************************************************************* */
35 TEST(dataSet, findExampleDataFile) {
36  const string expected_end = "examples/Data/example.graph";
37  const string actual = findExampleDataFile("example");
38  string actual_end = actual.substr(actual.size() - expected_end.size(), expected_end.size());
39  // replace all ocurrences of \\ with / use stl
40  std::replace(actual_end.begin(), actual_end.end(), '\\', '/');
41  EXPECT(assert_equal(expected_end, actual_end));
42 }
43 
44 /* ************************************************************************* */
45 TEST( dataSet, parseVertexPose)
46 {
47  const string str = "VERTEX2 1 2.000000 3.000000 4.000000";
48  istringstream is(str);
49  string tag;
50  EXPECT(is >> tag);
51  const auto actual = parseVertexPose(is, tag);
52  EXPECT(actual);
53  if (actual) {
54  EXPECT_LONGS_EQUAL(1, actual->first);
55  EXPECT(assert_equal(Pose2(2, 3, 4), actual->second));
56  }
57 }
58 
59 /* ************************************************************************* */
60 TEST( dataSet, parseVertexLandmark)
61 {
62  const string str = "VERTEX_XY 1 2.000000 3.000000";
63  istringstream is(str);
64  string tag;
65  EXPECT(is >> tag);
66  const auto actual = parseVertexLandmark(is, tag);
67  EXPECT(actual);
68  if (actual) {
69  EXPECT_LONGS_EQUAL(1, actual->first);
70  EXPECT(assert_equal(Point2(2, 3), actual->second));
71  }
72 }
73 
74 /* ************************************************************************* */
75 TEST( dataSet, parseEdge)
76 {
77  const string str = "EDGE2 0 1 2.000000 3.000000 4.000000";
78  istringstream is(str);
79  string tag;
80  EXPECT(is >> tag);
81  const auto actual = parseEdge(is, tag);
82  EXPECT(actual);
83  if (actual) {
84  pair<size_t, size_t> expected(0, 1);
85  EXPECT(expected == actual->first);
86  EXPECT(assert_equal(Pose2(2, 3, 4), actual->second));
87  }
88 }
89 
90 /* ************************************************************************* */
91 TEST(dataSet, load2D) {
93  const string filename = findExampleDataFile("w100.graph");
94  const auto [graph, initial] = load2D(filename);
95  EXPECT_LONGS_EQUAL(300, graph->size());
96  EXPECT_LONGS_EQUAL(100, initial->size());
97  auto model = noiseModel::Unit::Create(3);
98  BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879, 0.0417574, -0.00818381),
99  model);
100  BetweenFactor<Pose2>::shared_ptr actual = graph->at<BetweenFactor<Pose2>>(0);
101  EXPECT(assert_equal(expected, *actual));
102 
103  // Check binary measurements, Pose2
104  size_t maxIndex = 5;
105  auto measurements = parseMeasurements<Pose2>(filename, nullptr, maxIndex);
106  EXPECT_LONGS_EQUAL(5, measurements.size());
107 
108  // Check binary measurements, Rot2
109  auto measurements2 = parseMeasurements<Rot2>(filename);
110  EXPECT_LONGS_EQUAL(300, measurements2.size());
111 
112  // // Check factor parsing
113  const auto actualFactors = parseFactors<Pose2>(filename);
114  for (size_t i : {0, 1, 2, 3, 4, 5}) {
115  EXPECT(assert_equal(*graph->at<BetweenFactor<Pose2>>(i), *actualFactors[i],
116  1e-5));
117  }
118 
119  // Check pose parsing
120  const auto actualPoses = parseVariables<Pose2>(filename);
121  for (size_t j : {0, 1, 2, 3, 4}) {
122  EXPECT(assert_equal(initial->at<Pose2>(j), actualPoses.at(j), 1e-5));
123  }
124 
125  // Check landmark parsing
126  const auto actualLandmarks = parseVariables<Point2>(filename);
127  EXPECT_LONGS_EQUAL(0, actualLandmarks.size());
128 }
129 
130 /* ************************************************************************* */
131 TEST(dataSet, load2DVictoriaPark) {
132  const string filename = findExampleDataFile("victoria_park.txt");
133  // Load all
134  const auto [graph1, initial1] = load2D(filename);
135  EXPECT_LONGS_EQUAL(10608, graph1->size());
136  EXPECT_LONGS_EQUAL(7120, initial1->size());
137 
138  // Restrict keys
139  size_t maxIndex = 5;
140  const auto [graph2, initial2] = load2D(filename, nullptr, maxIndex);
141  EXPECT_LONGS_EQUAL(5, graph2->size());
142  EXPECT_LONGS_EQUAL(6, initial2->size()); // file has 0 as well
143  EXPECT_LONGS_EQUAL(L(5), graph2->at(4)->keys()[1]);
144 }
145 
146 /* ************************************************************************* */
147 TEST(dataSet, readG2o3D) {
148  const string g2oFile = findExampleDataFile("pose3example");
149  auto model = noiseModel::Isotropic::Precision(6, 10000);
150 
151  // Initialize 6 relative measurements with quaternion/point3 values:
152  const std::vector<Pose3> relative_poses = {
153  {{0.854230, 0.190253, 0.283162, -0.392318},
154  {1.001367, 0.015390, 0.004948}},
155  {{0.105373, 0.311512, 0.656877, -0.678505},
156  {0.523923, 0.776654, 0.326659}},
157  {{0.568551, 0.595795, -0.561677, 0.079353},
158  {0.910927, 0.055169, -0.411761}},
159  {{0.542221, -0.592077, 0.303380, -0.513226},
160  {0.775288, 0.228798, -0.596923}},
161  {{0.327419, -0.125250, -0.534379, 0.769122},
162  {-0.577841, 0.628016, -0.543592}},
163  {{0.083672, 0.104639, 0.627755, 0.766795},
164  {-0.623267, 0.086928, 0.773222}},
165  };
166 
167  // Initialize 5 poses with quaternion/point3 values:
168  const std::vector<Pose3> poses = {
169  {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}},
170  {{0.854230, 0.190253, 0.283162, -0.392318},
171  {1.001367, 0.015390, 0.004948}},
172  {{0.421446, -0.351729, -0.597838, 0.584174},
173  {1.993500, 0.023275, 0.003793}},
174  {{0.067024, 0.331798, -0.200659, 0.919323},
175  {2.004291, 1.024305, 0.018047}},
176  {{0.765488, -0.035697, -0.462490, 0.445933},
177  {0.999908, 1.055073, 0.020212}},
178  };
179 
180  // Specify connectivity:
181  using KeyPair = pair<Key, Key>;
182  std::vector<KeyPair> edges = {{0, 1}, {1, 2}, {2, 3}, {3, 4}, {1, 4}, {3, 0}};
183 
184  // Created expected factor graph
185  size_t i = 0;
186  NonlinearFactorGraph expectedGraph;
187  for (const auto& keys : edges) {
188  expectedGraph.emplace_shared<BetweenFactor<Pose3>>(
189  keys.first, keys.second, relative_poses[i++], model);
190  }
191 
192  // Check factor parsing
193  const auto actualFactors = parseFactors<Pose3>(g2oFile);
194  for (size_t i : {0, 1, 2, 3, 4, 5}) {
195  EXPECT(assert_equal(*expectedGraph.at<BetweenFactor<Pose3>>(i),
196  *actualFactors[i], 1e-5));
197  }
198 
199  // Check pose parsing
200  const auto actualPoses = parseVariables<Pose3>(g2oFile);
201  for (size_t j : {0, 1, 2, 3, 4}) {
202  EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
203  }
204 
205  // Check landmark parsing
206  const auto actualLandmarks = parseVariables<Point3>(g2oFile);
207  for (size_t j : {0, 1, 2, 3, 4}) {
208  EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
209  }
210 
211  // Check graph version
212  bool is3D = true;
213  const auto [actualGraph, actualValues] = readG2o(g2oFile, is3D);
214  EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5));
215  for (size_t j : {0, 1, 2, 3, 4}) {
216  EXPECT(assert_equal(poses[j], actualValues->at<Pose3>(j), 1e-5));
217  }
218 }
219 
220 /* ************************************************************************* */
221 TEST( dataSet, readG2o3DNonDiagonalNoise)
222 {
223  const string g2oFile = findExampleDataFile("pose3example-offdiagonal.txt");
224  bool is3D = true;
225  const auto [actualGraph, actualValues] = readG2o(g2oFile, is3D);
226 
227  Values expectedValues;
228  Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
229  Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
230  expectedValues.insert(0, Pose3(R0, p0));
231 
232  Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
233  Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
234  expectedValues.insert(1, Pose3(R1, p1));
235 
236  EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
237 
238  Matrix Info = Matrix(6,6);
239  for (int i = 0; i < 6; i++){
240  for (int j = i; j < 6; j++){
241  if(i==j)
242  Info(i, j) = 10000;
243  else{
244  Info(i, j) = i+1; // arbitrary nonzero number
245  Info(j, i) = i+1;
246  }
247  }
248  }
249  noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(Info.inverse());
250  NonlinearFactorGraph expectedGraph;
251  Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
252  Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
253  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(0, 1, Pose3(R01,p01), model);
254 
255  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2));
256 }
257 
258 /* ************************************************************************* */
259 TEST(dataSet, readG2oCheckDeterminants) {
260  const string g2oFile = findExampleDataFile("toyExample.g2o");
261 
262  // Check determinants in factors
263  auto factors = parseFactors<Pose3>(g2oFile);
264  EXPECT_LONGS_EQUAL(6, factors.size());
265  for (const auto& factor : factors) {
266  const Rot3 R = factor->measured().rotation();
267  EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9);
268  }
269 
270  // Check determinants in initial values
271  const map<size_t, Pose3> poses = parseVariables<Pose3>(g2oFile);
272  EXPECT_LONGS_EQUAL(5, poses.size());
273  for (const auto& key_value : poses) {
274  const Rot3 R = key_value.second.rotation();
275  EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9);
276  }
277  const map<size_t, Point3> landmarks = parseVariables<Point3>(g2oFile);
278  EXPECT_LONGS_EQUAL(0, landmarks.size());
279 }
280 
281 /* ************************************************************************* */
282 TEST(dataSet, readG2oLandmarks) {
283  const string g2oFile = findExampleDataFile("example_with_vertices.g2o");
284 
285  // Check number of poses and landmarks. Should be 8 each.
286  const map<size_t, Pose3> poses = parseVariables<Pose3>(g2oFile);
287  EXPECT_LONGS_EQUAL(8, poses.size());
288  const map<size_t, Point3> landmarks = parseVariables<Point3>(g2oFile);
289  EXPECT_LONGS_EQUAL(8, landmarks.size());
290 
291  auto graphAndValues = load3D(g2oFile);
292  EXPECT(graphAndValues.second->exists(L(0)));
293 }
294 
295 /* ************************************************************************* */
296 static NonlinearFactorGraph expectedGraph(const SharedNoiseModel& model) {
297  NonlinearFactorGraph g;
298  using Factor = BetweenFactor<Pose2>;
299  g.emplace_shared<Factor>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model);
300  g.emplace_shared<Factor>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model);
301  g.emplace_shared<Factor>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model);
302  g.emplace_shared<Factor>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model);
303  g.emplace_shared<Factor>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model);
304  g.emplace_shared<Factor>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model);
305  g.emplace_shared<Factor>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model);
306  g.emplace_shared<Factor>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model);
307  g.emplace_shared<Factor>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model);
308  g.emplace_shared<Factor>(9, 10, Pose2(1.003350, 0.022250, -0.195918), model);
309  g.emplace_shared<Factor>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model);
310  g.emplace_shared<Factor>(3, 10, Pose2(0.044020, 0.988477, -1.553511), model);
311  return g;
312 }
313 
314 /* ************************************************************************* */
315 TEST(dataSet, readG2o) {
316  const string g2oFile = findExampleDataFile("pose2example");
317  const auto [actualGraph, actualValues] = readG2o(g2oFile);
318 
319  auto model = noiseModel::Diagonal::Precisions(
320  Vector3(44.721360, 44.721360, 30.901699));
321  EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5));
322 
323  Values expectedValues;
324  expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000));
325  expectedValues.insert(1, Pose2(1.030390, 0.011350, -0.081596));
326  expectedValues.insert(2, Pose2(2.036137, -0.129733, -0.301887));
327  expectedValues.insert(3, Pose2(3.015097, -0.442395, -0.345514));
328  expectedValues.insert(4, Pose2(3.343949, 0.506678, 1.214715));
329  expectedValues.insert(5, Pose2(3.684491, 1.464049, 1.183785));
330  expectedValues.insert(6, Pose2(4.064626, 2.414783, 1.176333));
331  expectedValues.insert(7, Pose2(4.429778, 3.300180, 1.259169));
332  expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391));
333  expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016));
334  expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934));
335  EXPECT(assert_equal(expectedValues, *actualValues, 1e-5));
336 }
337 
338 /* ************************************************************************* */
339 TEST(dataSet, readG2oHuber) {
340  const string g2oFile = findExampleDataFile("pose2example");
341  bool is3D = false;
342  const auto [actualGraph, actualValues] =
343  readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
344 
345  auto baseModel = noiseModel::Diagonal::Precisions(
346  Vector3(44.721360, 44.721360, 30.901699));
347  auto model = noiseModel::Robust::Create(
348  noiseModel::mEstimator::Huber::Create(1.345), baseModel);
349 
350  EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5));
351 }
352 
353 /* ************************************************************************* */
354 TEST(dataSet, readG2oTukey) {
355  const string g2oFile = findExampleDataFile("pose2example");
356  bool is3D = false;
357  const auto [actualGraph, actualValues] =
358  readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
359 
360  auto baseModel = noiseModel::Diagonal::Precisions(
361  Vector3(44.721360, 44.721360, 30.901699));
362  auto model = noiseModel::Robust::Create(
363  noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
364 
365  EXPECT(assert_equal(expectedGraph(model), *actualGraph, 1e-5));
366 }
367 
368 /* ************************************************************************* */
369 TEST( dataSet, writeG2o)
370 {
371  const string g2oFile = findExampleDataFile("pose2example");
372  const auto [expectedGraph, expectedValues] = readG2o(g2oFile);
373 
374  const string filenameToWrite = createRewrittenFileName(g2oFile);
375  writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
376 
377  const auto [actualGraph, actualValues] = readG2o(filenameToWrite);
378  EXPECT(assert_equal(*expectedValues,*actualValues,1e-5));
379  EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5));
380 }
381 
382 /* ************************************************************************* */
383 TEST( dataSet, writeG2o3D)
384 {
385  const string g2oFile = findExampleDataFile("pose3example");
386  bool is3D = true;
387  const auto [expectedGraph, expectedValues] = readG2o(g2oFile, is3D);
388 
389  const string filenameToWrite = createRewrittenFileName(g2oFile);
390  writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
391 
392  const auto [actualGraph, actualValues] = readG2o(filenameToWrite, is3D);
393  EXPECT(assert_equal(*expectedValues,*actualValues,1e-4));
394  EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
395 }
396 
397 /* ************************************************************************* */
398 TEST( dataSet, writeG2o3DNonDiagonalNoise)
399 {
400  const string g2oFile = findExampleDataFile("pose3example-offdiagonal");
401  bool is3D = true;
402  const auto [expectedGraph, expectedValues] = readG2o(g2oFile, is3D);
403 
404  const string filenameToWrite = createRewrittenFileName(g2oFile);
405  writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
406 
407  const auto [actualGraph, actualValues] = readG2o(filenameToWrite, is3D);
408  EXPECT(assert_equal(*expectedValues,*actualValues,1e-4));
409  EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
410 }
411 
412 /* ************************************************************************* */
413 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
414 /* ************************************************************************* */
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
Pose2.h
2D Pose
gtsam::createRewrittenFileName
GTSAM_EXPORT std::string createRewrittenFileName(const std::string &name)
Definition: dataset.cpp:105
gtsam::noiseModel::mEstimator::Huber::Create
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
Definition: LossFunctions.cpp:203
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
EXPECT_LONGS_EQUAL
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
gtsam::noiseModel::Diagonal::Precisions
static shared_ptr Precisions(const Vector &precisions, bool smart=true)
Definition: NoiseModel.cpp:311
gtsam::Rot3::Quaternion
static Rot3 Quaternion(double w, double x, double y, double z)
Definition: Rot3.h:204
edges
vector< MFAS::KeyPair > edges
Definition: testMFAS.cpp:25
gtsam::noiseModel::Robust::Create
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
Definition: NoiseModel.cpp:740
initial
Values initial
Definition: OdometryOptimize.cpp:2
simple_graph::factors
const GaussianFactorGraph factors
Definition: testJacobianFactor.cpp:213
gtsam::readG2o
GraphAndValues readG2o(const std::string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
Definition: dataset.cpp:621
gtsam::symbol_shorthand
Definition: inference/Symbol.h:147
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::parseVariables< Pose2 >
GTSAM_EXPORT std::map< size_t, Pose2 > parseVariables< Pose2 >(const std::string &filename, size_t maxIndex)
Definition: dataset.cpp:187
gtsam::Factor
Definition: Factor.h:70
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::noiseModel::Isotropic::Precision
static shared_ptr Precision(size_t dim, double precision, bool smart=true)
Definition: NoiseModel.h:577
gtsam::noiseModel::Gaussian::shared_ptr
std::shared_ptr< Gaussian > shared_ptr
Definition: NoiseModel.h:191
expectedGraph
static NonlinearFactorGraph expectedGraph(const SharedNoiseModel &model)
Definition: testDataset.cpp:296
TestableAssertions.h
Provides additional testing facilities for common data structures.
so3::R1
SO3 R1
Definition: testShonanFactor.cpp:41
gtsam::parseVariables< Point2 >
GTSAM_EXPORT std::map< size_t, Point2 > parseVariables< Point2 >(const std::string &filename, size_t maxIndex)
Definition: dataset.cpp:209
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:306
simple::R0
static Rot3 R0
Definition: testInitializePose3.cpp:48
dataset.h
utility functions for loading datasets
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
initial2
Definition: testScenarioRunner.cpp:209
BetweenFactor.h
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
relicense.filename
filename
Definition: relicense.py:57
gtsam::Pose3
Definition: Pose3.h:37
gtsam::BetweenFactor::shared_ptr
std::shared_ptr< BetweenFactor > shared_ptr
Definition: BetweenFactor.h:63
main
int main()
Definition: testDataset.cpp:413
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:631
gtsam::parseVertexPose
std::optional< IndexedPose > parseVertexPose(std::istream &is, const std::string &tag)
Definition: dataset.cpp:173
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::symbol_shorthand::R
Key R(std::uint64_t j)
Definition: inference/Symbol.h:165
gtsam::KernelFunctionTypeHUBER
@ KernelFunctionTypeHUBER
Definition: dataset.h:75
Symbol.h
gtsam::symbol_shorthand::L
Key L(std::uint64_t j)
Definition: inference/Symbol.h:159
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
TestResult
Definition: TestResult.h:26
str
Definition: pytypes.h:1558
gtsam::FactorGraph::size
size_t size() const
Definition: FactorGraph.h:297
gtsam::load2D
GraphAndValues load2D(const std::string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Definition: dataset.cpp:505
gtsam
traits
Definition: SFMdata.h:40
gtsam::TEST
TEST(SmartFactorBase, Pinhole)
Definition: testSmartFactorBase.cpp:38
gtsam::KernelFunctionTypeTUKEY
@ KernelFunctionTypeTUKEY
Definition: dataset.h:75
switching3::graph2
const HybridGaussianFactorGraph graph2
Definition: testHybridGaussianISAM.cpp:53
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:51
gtsam::Values
Definition: Values.h:65
p0
Vector3f p0
Definition: MatrixBase_all.cpp:2
std
Definition: BFloat16.h:88
gtsam::parseVariables< Point3 >
GTSAM_EXPORT std::map< size_t, Point3 > parseVariables< Point3 >(const std::string &filename, size_t maxIndex)
Definition: dataset.cpp:793
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
switching3::graph1
const HybridGaussianFactorGraph graph1
Definition: testHybridGaussianISAM.cpp:50
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::parseFactors< Pose3 >
GTSAM_EXPORT std::vector< BetweenFactor< Pose3 >::shared_ptr > parseFactors< Pose3 >(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:914
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
initial
Definition: testScenarioRunner.cpp:148
gtsam::parseEdge
std::optional< IndexedEdge > parseEdge(std::istream &is, const std::string &tag)
Definition: dataset.cpp:299
gtsam::findExampleDataFile
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Definition: dataset.cpp:70
gtsam::noiseModel::mEstimator::Tukey::Create
static shared_ptr Create(double k, const ReweightScheme reweight=Block)
Definition: LossFunctions.cpp:279
gtsam::parseVariables< Pose3 >
GTSAM_EXPORT std::map< size_t, Pose3 > parseVariables< Pose3 >(const std::string &filename, size_t maxIndex)
Definition: dataset.cpp:775
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::load3D
GraphAndValues load3D(const std::string &filename)
Load TORO 3D Graph.
Definition: dataset.cpp:922
gtsam::parseVertexLandmark
std::optional< IndexedLandmark > parseVertexLandmark(std::istream &is, const std::string &tag)
Definition: dataset.cpp:193
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::noiseModel::Gaussian::Covariance
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
Definition: NoiseModel.cpp:114
gtsam::parseFactors< Pose2 >
GTSAM_EXPORT std::vector< BetweenFactor< Pose2 >::shared_ptr > parseFactors< Pose2 >(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
Definition: dataset.cpp:440
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::Pose2
Definition: Pose2.h:39
gtsam::BetweenFactor
Definition: BetweenFactor.h:40
gtsam::writeG2o
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure.
Definition: dataset.cpp:636

gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:49