From 94395249fb61e676fdd8e09a80630944eb970fee Mon Sep 17 00:00:00 2001
From: Kai Ni <nikai97@gmail.com>
Date: Fri, 2 Apr 2010 00:04:11 +0000
Subject: [PATCH] add stack_matrices to prevent the name conflict with class
 std::stack

---
 cpp/BearingRangeFactor.h | 22 ++++++++++++++++------
 cpp/Matrix.cpp           |  5 +++++
 cpp/Matrix.h             |  3 +++
 3 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/cpp/BearingRangeFactor.h b/cpp/BearingRangeFactor.h
index 4e8902fbd..da268caa7 100644
--- a/cpp/BearingRangeFactor.h
+++ b/cpp/BearingRangeFactor.h
@@ -40,12 +40,15 @@ namespace gtsam {
 		/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
 		Vector evaluateError(const Pose2& pose, const Point2& point,
 				boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-			boost::optional<Matrix&> H11 = H1, H21 = H1;
-			boost::optional<Matrix&> H12 = H2, H22 = H2;
-			Vector e1 = bearing_.evaluateError(pose, point, H11, H12);
-			Vector e2 =   range_.evaluateError(pose, point, H21, H21);
-			if (H1) *H1 = stack(2, &(*H11), &(*H21));
-			if (H2) *H2 = stack(2, &(*H12), &(*H22));
+			Matrix H11, H21, H12, H22;
+			boost::optional<Matrix&> H11_ = H1 ? boost::optional<Matrix&>(H11) : boost::optional<Matrix&>();
+			boost::optional<Matrix&> H21_ = H1 ? boost::optional<Matrix&>(H21) : boost::optional<Matrix&>();
+			boost::optional<Matrix&> H12_ = H2 ? boost::optional<Matrix&>(H12) : boost::optional<Matrix&>();
+			boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>();
+			Vector e1 = bearing_.evaluateError(pose, point, H11_, H12_);
+			Vector e2 =   range_.evaluateError(pose, point, H21_, H22_);
+			if (H1) *H1 = stack_matrices(H11, H21);
+			if (H2) *H2 = stack_matrices(H12, H22);
 			return concatVectors(2, &e1, &e2);
 		}
 
@@ -53,6 +56,13 @@ namespace gtsam {
 		inline const std::pair<Rot2, double> measured() const {
 			return concatVectors(2, bearing_.measured(), range_.measured());
 		}
+
+		/** return the bearing factor */
+		const BearingFactor<Config, PoseKey, PointKey>& bearing() const { return bearing_; }
+
+		/** return the range factor */
+		const RangeFactor<Config, PoseKey, PointKey>& range() const { return range_; }
+
 	}; // BearingRangeFactor
 
 } // namespace gtsam
diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp
index 6261952c4..765ef4d62 100644
--- a/cpp/Matrix.cpp
+++ b/cpp/Matrix.cpp
@@ -742,6 +742,11 @@ Matrix stack(size_t nrMatrices, ...)
   return A;
 }
 
+/* ************************************************************************* */
+Matrix stack_matrices(const Matrix& A, const Matrix& B) {
+	return stack(2, &A, &B);
+}
+
 /* ************************************************************************* */
 Matrix collect(const std::vector<const Matrix *>& matrices, size_t m, size_t n)
 {
diff --git a/cpp/Matrix.h b/cpp/Matrix.h
index 3bba01839..571c161aa 100644
--- a/cpp/Matrix.h
+++ b/cpp/Matrix.h
@@ -287,6 +287,9 @@ Vector backSubstituteLower(const Matrix& L, const Vector& d, bool unit=false);
  */
 Matrix stack(size_t nrMatrices, ...);
 
+/** a shortcut to prevent the name confliction with STL stack */
+Matrix stack_matrices(const Matrix& A, const Matrix& B);
+
 /**
  * create a matrix by concatenating
  * Given a set of matrices: A1, A2, A3...