From 3b10f61e5c426bc1e57493de1701d7ccd4c51ab0 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 27 May 2014 00:42:03 -0400
Subject: [PATCH] utilities.localToWorld

---
 gtsam.h                  |  2 ++
 matlab.h                 | 32 ++++++++++++++++++++++++++++++++
 matlab/+gtsam/Contents.m |  1 +
 3 files changed, 35 insertions(+)

diff --git a/gtsam.h b/gtsam.h
index 99aec3f86..96a778acf 100644
--- a/gtsam.h
+++ b/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
 
diff --git a/matlab.h b/matlab.h
index e9ad519bd..c4891a615 100644
--- a/matlab.h
+++ b/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;
+}
+
 }
 }
 
diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m
index 7a7491462..023c61dbe 100644
--- a/matlab/+gtsam/Contents.m
+++ b/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