Fix for Issue #201

It seems like MSVC was unable to identify the template specialization
for the 'const' keyword. So added a specialization in each of these
files for that

gtsam/geometry/Cal3Bundler.h

@@ -174,4 +174,7 @@ private:
 template<>
 struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
 
+template<>
+struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
+
 } // namespace gtsam

gtsam/geometry/Cal3DS2.h

@@ -112,5 +112,8 @@ private:
 template<>
 struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
 
+template<>
+struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
+
 }
 

gtsam/geometry/Cal3Unified.h

@@ -142,5 +142,8 @@ private:
 template<>
 struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
 
+template<>
+struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
+
 }
 

gtsam/geometry/Cal3_S2.h

@@ -226,4 +226,7 @@ private:
 template<>
 struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
 
+template<>
+struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
+
 } // \ namespace gtsam

gtsam/geometry/Cal3_S2Stereo.h

@@ -158,4 +158,8 @@ namespace gtsam {
   struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
   };
 
+  template<>
+  struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
+  };
+
 } // \ namespace gtsam

gtsam/geometry/CalibratedCamera.h

@@ -204,5 +204,8 @@ private:
 template<>
 struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
 
+template<>
+struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
+
 }
 

gtsam/geometry/EssentialMatrix.h

@@ -201,5 +201,8 @@ private:
 template<>
 struct traits<EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
 
+template<>
+struct traits<const EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
+
 } // gtsam
 

gtsam/geometry/PinholeCamera.h

@@ -499,4 +499,7 @@ private:
 template<typename Calibration>
 struct traits< PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
 
+template<typename Calibration>
+struct traits< const PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
+
 } // \ gtsam

gtsam/geometry/Point3.h

@@ -197,4 +197,7 @@ namespace gtsam {
 
   template<>
   struct traits<Point3> : public internal::VectorSpace<Point3> {};
+
+ template<>
+  struct traits<const Point3> : public internal::VectorSpace<Point3> {};
 }

gtsam/geometry/Pose2.h

@@ -293,5 +293,8 @@ GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
 template<>
 struct traits<Pose2> : public internal::LieGroupTraits<Pose2> {};
 
+template<>
+struct traits<const Pose2> : public internal::LieGroupTraits<Pose2> {};
+
 } // namespace gtsam
 

gtsam/geometry/Pose3.h

@@ -325,4 +325,8 @@ GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
 template<>
 struct traits<Pose3> : public internal::LieGroupTraits<Pose3> {};
 
+template<>
+struct traits<const Pose3> : public internal::LieGroupTraits<Pose3> {};
+
+
 } // namespace gtsam

gtsam/geometry/Rot2.h

@@ -210,4 +210,7 @@ namespace gtsam {
   template<>
   struct traits<Rot2> : public internal::LieGroupTraits<Rot2> {};
 
+  template<>
+  struct traits<const Rot2> : public internal::LieGroupTraits<Rot2> {};
+
 } // gtsam

gtsam/geometry/Rot3.h

@@ -463,5 +463,8 @@ namespace gtsam {
 
   template<>
   struct traits<Rot3> : public internal::LieGroupTraits<Rot3> {};
+  
+  template<>
+  struct traits<const Rot3> : public internal::LieGroupTraits<Rot3> {};
 }
 

gtsam/geometry/SO3.h

@@ -85,6 +85,7 @@ public:
 template<>
 struct traits<SO3> : public internal::LieGroupTraits<SO3> {};
 
-
+template<>
+struct traits<const SO3> : public internal::LieGroupTraits<SO3> {};
 } // end namespace gtsam
 

gtsam/geometry/StereoCamera.h

@@ -149,4 +149,6 @@ private:
 template<>
 struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {};
 
+template<>
+struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {};
 }

gtsam/geometry/StereoPoint2.h

@@ -176,4 +176,7 @@ namespace gtsam {
 
   template<>
   struct traits<StereoPoint2> : public internal::Manifold<StereoPoint2> {};
+
+  template<>
+  struct traits<const StereoPoint2> : public internal::Manifold<StereoPoint2> {};
 }

gtsam/geometry/Unit3.h

@@ -171,5 +171,7 @@ private:
 // Define GTSAM traits
 template <> struct traits<Unit3> : public internal::Manifold<Unit3> {};
 
+template <> struct traits<const Unit3> : public internal::Manifold<Unit3> {};
+
 } // namespace gtsam
 

gtsam/slam/dataset.cpp

@@ -19,6 +19,7 @@
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/BearingRangeFactor.h>
 #include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/linear/Sampler.h>
 #include <gtsam/inference/Symbol.h>