utilities.localToWorld

gtsam.h

@@ -2381,6 +2381,8 @@ namespace utilities {
   void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
   void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
   Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
+  gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base);
+  gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys);
 
 } //\namespace utilities
 

matlab.h

@@ -28,6 +28,8 @@
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/SimpleCamera.h>
 
+#include <boost/foreach.hpp>
+
 #include <exception>
 
 namespace gtsam {
@@ -218,6 +220,36 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
   return errors;
 }
 
+/// Convert from local to world coordinates
+Values localToWorld(const Values& local, const Pose2& base,
+    const FastVector<Key> user_keys = FastVector<Key>()) {
+
+  Values world;
+
+  // if no keys given, get all keys from local values
+  FastVector<Key> keys(user_keys);
+  if (keys.size()==0)
+    keys = FastVector<Key>(local.keys());
+
+  // Loop over all keys
+  BOOST_FOREACH(Key key, keys) {
+    try {
+      // if value is a Pose2, compose it with base pose
+      Pose2 pose = local.at<Pose2>(key);
+      world.insert(key, base.compose(pose));
+    } catch (std::exception e1) {
+      try {
+        // if value is a Point2, transform it from base pose
+        Point2 point = local.at<Point2>(key);
+        world.insert(key, base.transform_from(point));
+      } catch (std::exception e2) {
+        // if not Pose2 or Point2, do nothing
+      }
+    }
+  }
+  return world;
+}
+
 }
 }
 

matlab/+gtsam/Contents.m

@@ -189,3 +189,4 @@
 %   utilities.insertBackprojections   - Insert a number of initial point values by backprojecting
 %   utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key
 %   utilities.reprojectionErrors      - Calculate the errors of all projection factors in a graph
+%   utilities.localToWorld            - Convert from local to world coordinates