diff --git a/cpp/UrbanConfig.cpp b/cpp/UrbanConfig.cpp
index 62227a48a..f62edf79e 100644
--- a/cpp/UrbanConfig.cpp
+++ b/cpp/UrbanConfig.cpp
@@ -86,19 +86,13 @@ bool UrbanConfig::equals(const UrbanConfig& c, double tol) const {
 /* ************************************************************************* */
 void UrbanConfig::addRobotPose(const int i, Pose3 cp)
 {
-  pair<int, Pose3> robot;
-  robot.first = i;
-  robot.second = cp;
-  robotPoses_.insert(robot);
+  robotPoses_.insert(make_pair(i,cp));
 }
 
 /* ************************************************************************* */
-void UrbanConfig::addLandmark(const int i, Point2 lp)
+void UrbanConfig::addLandmark(const int j, Point2 lp)
 {
-  pair<int, Point2> landmark;
-  landmark.first = i;
-  landmark.second = lp;
-  landmarkPoints_.insert(landmark);
+  landmarkPoints_.insert(make_pair(j,lp));
 }
 
 /* ************************************************************************* */
diff --git a/cpp/UrbanConfig.h b/cpp/UrbanConfig.h
index fff15dbc5..b698b0517 100644
--- a/cpp/UrbanConfig.h
+++ b/cpp/UrbanConfig.h
@@ -6,6 +6,7 @@
  */
 
 #include <map>
+#include <sstream>
 #include "Testable.h"
 #include "VectorConfig.h"
 #include "Point2.h"
@@ -13,106 +14,120 @@
 
 #pragma once
 
-namespace gtsam{
-
-/**
- * Config that knows about points and poses
- */
-class UrbanConfig : Testable<UrbanConfig> {
-
- private:
-  typedef std::map<int, Pose3> PoseMap;
-  typedef std::map<int, Point2> PointMap;
-  PointMap landmarkPoints_;
-  PoseMap robotPoses_;
-
- public:
-  typedef std::map<std::string, Vector>::const_iterator const_iterator;
-  typedef PoseMap::const_iterator const_Pose_iterator;
-  typedef PointMap::const_iterator const_Point_iterator;
-  /**
-   * default constructor
-   */
-  UrbanConfig() {}
-
-  /*
-   * copy constructor
-   */
-  UrbanConfig(const UrbanConfig& original):
-  	robotPoses_(original.robotPoses_), landmarkPoints_(original.landmarkPoints_){}
+namespace gtsam {
 
 	/**
-	 * Exponential map: takes 6D vectors in VectorConfig
-	 * and applies them to the poses in the UrbanConfig.
-	 * Needed for use in nonlinear optimization
+	 * Config that knows about points and poses
 	 */
-  UrbanConfig exmap(const VectorConfig & delta) const;
+	class UrbanConfig: Testable<UrbanConfig> {
 
-  PoseMap::const_iterator robotIteratorBegin() const  { return robotPoses_.begin();}
-  PoseMap::const_iterator robotIteratorEnd() const   { return robotPoses_.end();}
-  PointMap::const_iterator landmarkIteratorBegin() const { return landmarkPoints_.begin();}
-  PointMap::const_iterator landmarkIteratorEnd() const  { return landmarkPoints_.end();}
+	private:
+		typedef std::map<int, Pose3> PoseMap;
+		typedef std::map<int, Point2> PointMap;
+		PointMap landmarkPoints_;
+		PoseMap robotPoses_;
 
-  /**
-   * print
-   */
-  void print(const std::string& s = "") const;
+		static std::string symbol(char c, int index) {
+			std::stringstream ss;
+			ss << c << index;
+			return ss.str();
+		}
 
-  /**
-   * Retrieve robot pose
-   */
-  bool robotPoseExists(int i) const
-  {
-    PoseMap::const_iterator it = robotPoses_.find(i);
-    if (it==robotPoses_.end())
-      return false;
-    return true;
-  }
+	public:
+		typedef std::map<std::string, Vector>::const_iterator const_iterator;
+		typedef PoseMap::const_iterator const_Pose_iterator;
+		typedef PointMap::const_iterator const_Point_iterator;
+		/**
+		 * default constructor
+		 */
+		UrbanConfig() {
+		}
 
-  Pose3 robotPose(int i) const {
-    PoseMap::const_iterator it = robotPoses_.find(i);
-    if (it==robotPoses_.end())
-      throw(std::invalid_argument("robotPose: invalid key"));
-    return it->second;
-  }
+		/*
+		 * copy constructor
+		 */
+		UrbanConfig(const UrbanConfig& original) :
+			robotPoses_(original.robotPoses_), landmarkPoints_(
+					original.landmarkPoints_) {
+		}
 
-  /**
-   * Check whether a landmark point exists
-   */
-  bool landmarkPointExists(int i) const
-  {
-    PointMap::const_iterator it = landmarkPoints_.find(i);
-    if (it==landmarkPoints_.end())
-      return false;
-    return true;
-  }
+		/**
+		 * Exponential map: takes 6D vectors in VectorConfig
+		 * and applies them to the poses in the UrbanConfig.
+		 * Needed for use in nonlinear optimization
+		 */
+		UrbanConfig exmap(const VectorConfig & delta) const;
 
-  /**
-   * Retrieve landmark point
-   */
-  Point2 landmarkPoint(int i) const {
-    PointMap::const_iterator it = landmarkPoints_.find(i);
-    if (it==landmarkPoints_.end())
-      throw(std::invalid_argument("markerPose: invalid key"));
-    return it->second;
-  }
+		PoseMap::const_iterator robotIteratorBegin() const {
+			return robotPoses_.begin();
+		}
+		PoseMap::const_iterator robotIteratorEnd() const {
+			return robotPoses_.end();
+		}
+		PointMap::const_iterator landmarkIteratorBegin() const {
+			return landmarkPoints_.begin();
+		}
+		PointMap::const_iterator landmarkIteratorEnd() const {
+			return landmarkPoints_.end();
+		}
 
-  /**
-   * check whether two configs are equal
-   */
-  bool equals(const UrbanConfig& c, double tol=1e-6) const;
-  void addRobotPose(const int i, Pose3 cp);
-  void addLandmark(const int i, Point2 lp);
+		/**
+		 * print
+		 */
+		void print(const std::string& s = "") const;
 
-  void removeRobotPose(const int i);
-  void removeLandmark(const int i);
+		/**
+		 * Retrieve robot pose
+		 */
+		bool robotPoseExists(int i) const {
+			PoseMap::const_iterator it = robotPoses_.find(i);
+			return (it != robotPoses_.end());
+		}
 
-  void clear() {landmarkPoints_.clear(); robotPoses_.clear();}
+		Pose3 robotPose(int i) const {
+			PoseMap::const_iterator it = robotPoses_.find(i);
+			if (it == robotPoses_.end()) throw(std::invalid_argument(
+					"robotPose: invalid key " + symbol('x',i)));
+			return it->second;
+		}
 
-  inline size_t size(){
-    return landmarkPoints_.size() + robotPoses_.size();
-  }
-};
+		/**
+		 * Check whether a landmark point exists
+		 */
+		bool landmarkPointExists(int i) const {
+			PointMap::const_iterator it = landmarkPoints_.find(i);
+			return (it != landmarkPoints_.end());
+		}
+
+		/**
+		 * Retrieve landmark point
+		 */
+		Point2 landmarkPoint(int i) const {
+			PointMap::const_iterator it = landmarkPoints_.find(i);
+			if (it == landmarkPoints_.end()) throw(std::invalid_argument(
+					"markerPose: invalid key " + symbol('l',i)));
+			return it->second;
+		}
+
+		/**
+		 * check whether two configs are equal
+		 */
+		bool equals(const UrbanConfig& c, double tol = 1e-6) const;
+		void addRobotPose(const int i, Pose3 cp);
+		void addLandmark(const int i, Point2 lp);
+
+		void removeRobotPose(const int i);
+		void removeLandmark(const int i);
+
+		void clear() {
+			landmarkPoints_.clear();
+			robotPoses_.clear();
+		}
+
+		inline size_t size() {
+			return landmarkPoints_.size() + robotPoses_.size();
+		}
+	};
 
 } // namespace gtsam