Added a print function for RangeFactor

gtsam/slam/RangeFactor.h

@@ -16,8 +16,7 @@
 
 #pragma once
 
-//#include <gtsam/geometry/Pose2.h>
-//#include <gtsam/geometry/Point2.h>
+#include <boost/lexical_cast.hpp>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace gtsam {
@@ -63,6 +62,11 @@ namespace gtsam {
 			return e != NULL && Base::equals(*e, tol) && fabs(this->z_ - e->z_) < tol;
 		}
 
+		/** print contents */
+		void print(const std::string& s="") const {
+			Base::print(s + std::string(" range: ") + boost::lexical_cast<std::string>(z_));
+		}
+
 	private:
 
 		/** Serialization function */