better error message, formatting

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;
+  robotPoses_.insert(make_pair(i,cp));
-  robot.first = i;
-  robot.second = cp;
-  robotPoses_.insert(robot);
 }
 
 /* ************************************************************************* */
-void UrbanConfig::addLandmark(const int i, Point2 lp)
+void UrbanConfig::addLandmark(const int j, Point2 lp)
 {
-  pair<int, Point2> landmark;
+  landmarkPoints_.insert(make_pair(j,lp));
-  landmark.first = i;
-  landmark.second = lp;
-  landmarkPoints_.insert(landmark);
 }
 
 /* ************************************************************************* */

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{
+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_){}
 
 	/**
-	 * Exponential map: takes 6D vectors in VectorConfig
+	 * Config that knows about points and poses
-	 * and applies them to the poses in the UrbanConfig.
-	 * Needed for use in nonlinear optimization
 	 */
-  UrbanConfig exmap(const VectorConfig & delta) const;
+	class UrbanConfig: Testable<UrbanConfig> {
 
-  PoseMap::const_iterator robotIteratorBegin() const  { return robotPoses_.begin();}
+	private:
-  PoseMap::const_iterator robotIteratorEnd() const   { return robotPoses_.end();}
+		typedef std::map<int, Pose3> PoseMap;
-  PointMap::const_iterator landmarkIteratorBegin() const { return landmarkPoints_.begin();}
+		typedef std::map<int, Point2> PointMap;
-  PointMap::const_iterator landmarkIteratorEnd() const  { return landmarkPoints_.end();}
+		PointMap landmarkPoints_;
+		PoseMap robotPoses_;
 
-  /**
+		static std::string symbol(char c, int index) {
-   * print
+			std::stringstream ss;
-   */
+			ss << c << index;
-  void print(const std::string& s = "") const;
+			return ss.str();
+		}
 
-  /**
+	public:
-   * Retrieve robot pose
+		typedef std::map<std::string, Vector>::const_iterator const_iterator;
-   */
+		typedef PoseMap::const_iterator const_Pose_iterator;
-  bool robotPoseExists(int i) const
+		typedef PointMap::const_iterator const_Point_iterator;
-  {
+		/**
-    PoseMap::const_iterator it = robotPoses_.find(i);
+		 * default constructor
-    if (it==robotPoses_.end())
+		 */
-      return false;
+		UrbanConfig() {
-    return true;
+		}
-  }
 
-  Pose3 robotPose(int i) const {
+		/*
-    PoseMap::const_iterator it = robotPoses_.find(i);
+		 * copy constructor
-    if (it==robotPoses_.end())
+		 */
-      throw(std::invalid_argument("robotPose: invalid key"));
+		UrbanConfig(const UrbanConfig& original) :
-    return it->second;
+			robotPoses_(original.robotPoses_), landmarkPoints_(
-  }
+					original.landmarkPoints_) {
+		}
 
-  /**
+		/**
-   * Check whether a landmark point exists
+		 * Exponential map: takes 6D vectors in VectorConfig
-   */
+		 * and applies them to the poses in the UrbanConfig.
-  bool landmarkPointExists(int i) const
+		 * Needed for use in nonlinear optimization
-  {
+		 */
-    PointMap::const_iterator it = landmarkPoints_.find(i);
+		UrbanConfig exmap(const VectorConfig & delta) const;
-    if (it==landmarkPoints_.end())
-      return false;
-    return true;
-  }
 
-  /**
+		PoseMap::const_iterator robotIteratorBegin() const {
-   * Retrieve landmark point
+			return robotPoses_.begin();
-   */
+		}
-  Point2 landmarkPoint(int i) const {
+		PoseMap::const_iterator robotIteratorEnd() const {
-    PointMap::const_iterator it = landmarkPoints_.find(i);
+			return robotPoses_.end();
-    if (it==landmarkPoints_.end())
+		}
-      throw(std::invalid_argument("markerPose: invalid key"));
+		PointMap::const_iterator landmarkIteratorBegin() const {
-    return it->second;
+			return landmarkPoints_.begin();
-  }
+		}
+		PointMap::const_iterator landmarkIteratorEnd() const {
+			return landmarkPoints_.end();
+		}
 
-  /**
+		/**
-   * check whether two configs are equal
+		 * print
-   */
+		 */
-  bool equals(const UrbanConfig& c, double tol=1e-6) const;
+		void print(const std::string& s = "") 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);
+		 * 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