Removed filter from examples and timing scripts

examples/StereoVOExample_large.cpp

@@ -27,6 +27,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/utilities.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -113,7 +114,7 @@ int main(int argc, char** argv) {
   Values result = optimizer.optimize();
 
   cout << "Final result sample:" << endl;
-  Values pose_values = result.filter<Pose3>();
+  Values pose_values = utilities::allPose3s(result);
   pose_values.print("Final camera poses:\n");
 
   return 0;

gtsam_unstable/examples/ConcurrentCalibration.cpp

@@ -20,6 +20,7 @@
 
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/utilities.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -35,12 +36,15 @@
 
 using namespace std;
 using namespace gtsam;
+using symbol_shorthand::K;
+using symbol_shorthand::L;
+using symbol_shorthand::X;
 
 int main(int argc, char** argv){
 
   Values initial_estimate;
   NonlinearFactorGraph graph;
-  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,1);
+  const auto model = noiseModel::Isotropic::Sigma(2,1);
 
   string calibration_loc = findExampleDataFile("VO_calibration00s.txt");
   string pose_loc = findExampleDataFile("VO_camera_poses00s.txt");
@@ -54,13 +58,13 @@ int main(int argc, char** argv){
   calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
 
   //create stereo camera calibration object
-  const Cal3_S2::shared_ptr K(new Cal3_S2(fx,fy,s,u0,v0));
+  const Cal3_S2 true_K(fx,fy,s,u0,v0);
-  const Cal3_S2::shared_ptr noisy_K(new Cal3_S2(fx*1.2,fy*1.2,s,u0-10,v0+10));
+  const Cal3_S2 noisy_K(fx*1.2,fy*1.2,s,u0-10,v0+10);
 
-  initial_estimate.insert(Symbol('K', 0), *noisy_K);
+  initial_estimate.insert(K(0), noisy_K);
 
-  noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished());
+  auto calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished());
-  graph.addPrior(Symbol('K', 0), *noisy_K, calNoise);
+  graph.addPrior(K(0), noisy_K, calNoise);
 
 
   ifstream pose_file(pose_loc.c_str());
@@ -75,7 +79,7 @@ int main(int argc, char** argv){
     initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
   }
 
-  noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01);
+  auto poseNoise = noiseModel::Isotropic::Sigma(6, 0.01);
   graph.addPrior(Symbol('x', pose_id), Pose3(m), poseNoise);
 
   // camera and landmark keys
@@ -83,22 +87,22 @@ int main(int argc, char** argv){
 
   // pixel coordinates uL, uR, v (same for left/right images due to rectification)
   // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
-  double uL, uR, v, X, Y, Z;
+  double uL, uR, v, _X, Y, Z;
   ifstream factor_file(factor_loc.c_str());
   cout << "Reading stereo factors" << endl;
   //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
-  while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
+  while (factor_file >> x >> l >> uL >> uR >> v >> _X >> Y >> Z) {
-//    graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K);
+//    graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model, X(x), L(l), K);
 
-    graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0));
+    graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(Point2(uL,v), model, X(x), L(l), K(0));
 
 
     //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
-    if (!initial_estimate.exists(Symbol('l', l))) {
+    if (!initial_estimate.exists(L(l))) {
-      Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
+      Pose3 camPose = initial_estimate.at<Pose3>(X(x));
       //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space
-      Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z));
+      Point3 worldPoint = camPose.transformFrom(Point3(_X, Y, Z));
-      initial_estimate.insert(Symbol('l', l), worldPoint);
+      initial_estimate.insert(L(l), worldPoint);
     }
   }
 
@@ -123,7 +127,7 @@ int main(int argc, char** argv){
   double currentError;
 
 
-  stream_K << optimizer.iterations() << " " << optimizer.values().at<Cal3_S2>(Symbol('K',0)).vector().transpose() << endl;
+  stream_K << optimizer.iterations() << " " << optimizer.values().at<Cal3_S2>(K(0)).vector().transpose() << endl;
 
 
   // Iterative loop
@@ -132,7 +136,7 @@ int main(int argc, char** argv){
     currentError = optimizer.error();
     optimizer.iterate();
 
-    stream_K << optimizer.iterations() << " " << optimizer.values().at<Cal3_S2>(Symbol('K',0)).vector().transpose() << endl;
+    stream_K << optimizer.iterations() << " " << optimizer.values().at<Cal3_S2>(K(0)).vector().transpose() << endl;
 
     if(params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "newError: " << optimizer.error() << endl;
   } while(optimizer.iterations() < params.maxIterations &&
@@ -142,13 +146,13 @@ int main(int argc, char** argv){
   Values result = optimizer.values();
 
   cout << "Final result sample:" << endl;
-  Values pose_values = result.filter<Pose3>();
+  Values pose_values = utilities::allPose3s(result);
   pose_values.print("Final camera poses:\n");
 
-  Values(result.filter<Cal3_S2>()).print("Final K\n");
+  result.at<Cal3_S2>(K(0)).print("Final K\n");
 
-  noisy_K->print("Initial noisy K\n");
+  noisy_K.print("Initial noisy K\n");
-  K->print("Initial correct K\n");
+  true_K.print("Initial correct K\n");
 
   return 0;
 }

gtsam_unstable/examples/SmartProjectionFactorExample.cpp

@@ -30,6 +30,7 @@
 #include <gtsam/slam/dataset.h>
 #include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/utilities.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -114,7 +115,7 @@ int main(int argc, char** argv){
   Values result = optimizer.optimize();
 
   cout << "Final result sample:" << endl;
-  Values pose_values = result.filter<Pose3>();
+  Values pose_values = utilities::allPose3s(result);
   pose_values.print("Final camera poses:\n");
 
   return 0;

gtsam_unstable/examples/SmartRangeExample_plaza1.cpp

@@ -234,9 +234,8 @@ int main(int argc, char** argv) {
         }
       }
       countK = 0;
-      for(const auto it: result.filter<Point2>())
+      for (const auto& [key, point] : result.extract<Point2>())
-        os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
+        os2 << key << "\t" << point.x() << "\t" << point.y() << "\t1" << endl;
-            << endl;
       if (smart) {
         for(size_t jj: ids) {
           Point2 landmark = smartFactors[jj]->triangulate(result);
@@ -257,9 +256,8 @@ int main(int argc, char** argv) {
   // Write result to file
   Values result = isam.calculateEstimate();
   ofstream os("rangeResult.txt");
-  for(const auto it: result.filter<Pose2>())
+  for (const auto& [key, pose] : result.extract<Pose2>())
-    os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
+    os << key << "\t" << pose.x() << "\t" << pose.y() << "\t" << pose.theta() << endl;
-        << it.value.theta() << endl;
   exit(0);
 }
 

gtsam_unstable/examples/SmartRangeExample_plaza2.cpp

@@ -202,13 +202,11 @@ int main(int argc, char** argv) {
   // Write result to file
   Values result = isam.calculateEstimate();
   ofstream os2("rangeResultLM.txt");
-  for(const auto it: result.filter<Point2>())
+  for (const auto& [key, point] : result.extract<Point2>())
-    os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
+    os2 << key << "\t" << point.x() << "\t" << point.y() << "\t1" << endl;
-        << endl;
   ofstream os("rangeResult.txt");
-  for(const auto it: result.filter<Pose2>())
+  for (const auto& [key, pose] : result.extract<Pose2>())
-    os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
+    os << key << "\t" << pose.x() << "\t" << pose.y() << "\t" << pose.theta() << endl;
-        << it.value.theta() << endl;
   exit(0);
 }
 

gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp

@@ -29,6 +29,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/utilities.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -43,6 +44,7 @@
 
 using namespace std;
 using namespace gtsam;
+using symbol_shorthand::X;
 
 int main(int argc, char** argv){
 
@@ -84,16 +86,16 @@ int main(int argc, char** argv){
     if(add_initial_noise){
       m(1,3) += (pose_id % 10)/10.0;
     }
-    initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
+    initial_estimate.insert(X(pose_id), Pose3(m));
   }
 
-  Values initial_pose_values = initial_estimate.filter<Pose3>();
+  const auto initialPoses = initial_estimate.extract<Pose3>();
   if (output_poses) {
     init_pose3Out.open(init_poseOutput.c_str(), ios::out);
-    for (size_t i = 1; i <= initial_pose_values.size(); i++) {
+    for (size_t i = 1; i <= initialPoses.size(); i++) {
       init_pose3Out
           << i << " "
-          << initial_pose_values.at<Pose3>(Symbol('x', i)).matrix().format(
+          << initialPoses.at(X(i)).matrix().format(
               Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl;
     }
   }
@@ -103,7 +105,7 @@ int main(int argc, char** argv){
 
   // pixel coordinates uL, uR, v (same for left/right images due to rectification)
   // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
-  double uL, uR, v, X, Y, Z;
+  double uL, uR, v, _X, Y, Z;
   ifstream factor_file(factor_loc.c_str());
   cout << "Reading stereo factors" << endl;
 
@@ -112,21 +114,21 @@ int main(int argc, char** argv){
   SmartFactor::shared_ptr factor(new SmartFactor(model));
   size_t current_l = 3;   // hardcoded landmark ID from first measurement
 
-  while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
+  while (factor_file >> x >> l >> uL >> uR >> v >> _X >> Y >> Z) {
 
     if(current_l != l) {
       graph.push_back(factor);
       factor = SmartFactor::shared_ptr(new SmartFactor(model));
       current_l = l;
     }
-    factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), K);
+    factor->add(StereoPoint2(uL,uR,v), X(x), K);
   }
 
-  Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
+  Pose3 first_pose = initial_estimate.at<Pose3>(X(1));
   //constrain the first pose such that it cannot change from its original value during optimization
   // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
   // QR is much slower than Cholesky, but numerically more stable
-  graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
+  graph.emplace_shared<NonlinearEquality<Pose3> >(X(1),first_pose);
 
   LevenbergMarquardtParams params;
   params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
@@ -138,13 +140,13 @@ int main(int argc, char** argv){
   Values result = optimizer.optimize();
 
   cout << "Final result sample:" << endl;
-  Values pose_values = result.filter<Pose3>();
+  Values pose_values = utilities::allPose3s(result);
   pose_values.print("Final camera poses:\n");
 
   if(output_poses){
     pose3Out.open(poseOutput.c_str(),ios::out);
     for(size_t i = 1; i<=pose_values.size(); i++){
-      pose3Out << i << " " << pose_values.at<Pose3>(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
+      pose3Out << i << " " << pose_values.at<Pose3>(X(i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0,
         " ", " ")) << endl;
     }
     cout << "Writing output" << endl;

timing/timeLago.cpp

@@ -10,8 +10,8 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    timeVirtual.cpp
+ * @file    timeLago.cpp
- * @brief   Time the overhead of using virtual destructors and methods
+ * @brief   Time the LAGO initialization method
  * @author  Richard Roberts
  * @date    Dec 3, 2010
  */
@@ -36,16 +36,15 @@ int main(int argc, char *argv[]) {
   Values::shared_ptr solution;
   NonlinearFactorGraph::shared_ptr g;
   string inputFile = findExampleDataFile("w10000");
-  SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished());
+  auto model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished());
   boost::tie(g, solution) = load2D(inputFile, model);
 
   // add noise to create initial estimate
   Values initial;
-  Values::ConstFiltered<Pose2> poses = solution->filter<Pose2>();
+  auto noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished());
-  SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished());
   Sampler sampler(noise);
-  for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: poses)
+  for(const auto& [key,pose]: solution->extract<Pose2>())
-    initial.insert(it.key, it.value.retract(sampler.sample()));
+    initial.insert(key, pose.retract(sampler.sample()));
 
   // Add prior on the pose having index (key) = 0
   noiseModel::Diagonal::shared_ptr priorModel = //