From 0ca3f9d199e8900f119248a64a71694af27bcf17 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@cc.gatech.edu>
Date: Wed, 13 Mar 2019 23:22:37 -0400
Subject: [PATCH] Use c+11 initializer lists

---
 gtsam/slam/tests/testDataset.cpp | 91 +++++++++++++++-----------------
 1 file changed, 44 insertions(+), 47 deletions(-)

diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp
index 39c2d3722..4a327b8e1 100644
--- a/gtsam/slam/tests/testDataset.cpp
+++ b/gtsam/slam/tests/testDataset.cpp
@@ -162,65 +162,62 @@ TEST( dataSet, readG2o)
 }
 
 /* ************************************************************************* */
-TEST( dataSet, readG2o3D)
-{
+TEST(dataSet, readG2o3D) {
   const string g2oFile = findExampleDataFile("pose3example");
   NonlinearFactorGraph::shared_ptr actualGraph;
   Values::shared_ptr actualValues;
   bool is3D = true;
   boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
 
-  Values expectedValues;
-  Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
-  Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
-  expectedValues.insert(0, Pose3(R0, p0));
+  // Initialize 5 poses with quaternion/point3 values:
+  const std::vector<Pose3> poses = {
+      {{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}},
+      {{0.854230, 0.190253, 0.283162, -0.392318},
+       {1.001367, 0.015390, 0.004948}},
+      {{0.421446, -0.351729, -0.597838, 0.584174},
+       {1.993500, 0.023275, 0.003793}},
+      {{0.067024, 0.331798, -0.200659, 0.919323},
+       {2.004291, 1.024305, 0.018047}},
+      {{0.765488, -0.035697, -0.462490, 0.445933},
+       {0.999908, 1.055073, 0.020212}},
+  };
 
-  Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
-  Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
-  expectedValues.insert(1, Pose3(R1, p1));
+  // Check values
+  for (size_t i : {0, 1, 2, 3, 4}) {
+    EXPECT(assert_equal(poses[i], actualValues->at<Pose3>(i), 1e-5));
+  }
 
-  Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 );
-  Point3 p2 = Point3(1.993500, 0.023275, 0.003793);
-  expectedValues.insert(2, Pose3(R2, p2));
+  // Initialize 6 relative measurements with quaternion/point3 values:
+  const std::vector<Pose3> measurements = {
+      {{0.854230, 0.190253, 0.283162, -0.392318},
+       {1.001367, 0.015390, 0.004948}},
+      {{0.105373, 0.311512, 0.656877, -0.678505},
+       {0.523923, 0.776654, 0.326659}},
+      {{0.568551, 0.595795, -0.561677, 0.079353},
+       {0.910927, 0.055169, -0.411761}},
+      {{0.542221, -0.592077, 0.303380, -0.513226},
+       {0.775288, 0.228798, -0.596923}},
+      {{0.327419, -0.125250, -0.534379, 0.769122},
+       {-0.577841, 0.628016, -0.543592}},
+      {{0.083672, 0.104639, 0.627755, 0.766795},
+       {-0.623267, 0.086928, 0.773222}},
+  };
 
-  Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323);
-  Point3 p3 = Point3(2.004291, 1.024305, 0.018047);
-  expectedValues.insert(3, Pose3(R3, p3));
+  // Specify connectivity:
+  std::vector<pair<Key, Key>> pairs = {{0, 1}, {1, 2}, {2, 3},
+                                       {3, 4}, {1, 4}, {3, 0}};
 
-  Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933);
-  Point3 p4 = Point3(0.999908, 1.055073, 0.020212);
-  expectedValues.insert(4, Pose3(R4, p4));
-
-  EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
-
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0).finished());
+  // Create expected factor graph
+  auto model = noiseModel::Isotropic::Precision(6, 10000);
   NonlinearFactorGraph expectedGraph;
+  size_t i = 0;
+  for (const auto& keys : pairs) {
+    expectedGraph.emplace_shared<BetweenFactor<Pose3>>(
+        keys.first, keys.second, measurements[i++], model);
+  }
 
-  Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
-  Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(0, 1, Pose3(R01,p01), model);
-
-  Point3 p12 = Point3(0.523923, 0.776654, 0.326659);
-  Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, Pose3(R12,p12), model);
-
-  Point3 p23 = Point3(0.910927, 0.055169, -0.411761);
-  Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, Pose3(R23,p23), model);
-
-  Point3 p34 = Point3(0.775288, 0.228798, -0.596923);
-  Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, Pose3(R34,p34), model);
-
-  Point3 p14 = Point3(-0.577841, 0.628016, -0.543592);
-  Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122  );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 4, Pose3(R14,p14), model);
-
-  Point3 p30 = Point3(-0.623267, 0.086928, 0.773222);
-  Rot3 R30 = Rot3::Quaternion(0.083672 ,  0.104639, 0.627755, 0.766795  );
-  expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 0, Pose3(R30,p30), model);
-
-  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
+  // Check if actual graph is correct
+  EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5));
 }
 
 /* ************************************************************************* */