Extend template values with Similarity2 and Similarity3

Author: maciejhalber

gtsam/nonlinear/nonlinear.i

@@ -17,6 +17,8 @@ namespace gtsam {
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Similarity2.h>
+#include <gtsam/geometry/Similarity3.h>
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/SO3.h>
@@ -74,6 +76,8 @@ class NonlinearFactorGraph {
                  gtsam::Rot3,
                  gtsam::Pose2,
                  gtsam::Pose3,
+                 gtsam::Similarity2,
+                 gtsam::Similarity3,
                  gtsam::Cal3_S2,
                  gtsam::Cal3f,
                  gtsam::Cal3Bundler,
@@ -544,7 +548,7 @@ class ISAM2 {
   bool valueExists(gtsam::Key key) const;
   gtsam::Values calculateEstimate() const;
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
-                     gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
+                     gtsam::Rot3, gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Cal3_S2, gtsam::Cal3DS2,
                      gtsam::Cal3f, gtsam::Cal3Bundler, 
                      gtsam::EssentialMatrix, gtsam::FundamentalMatrix, gtsam::SimpleFundamentalMatrix,
                      gtsam::PinholeCamera<gtsam::Cal3_S2>,
@@ -609,6 +613,8 @@ template <T = {double,
                gtsam::Rot3,
                gtsam::Pose2,
                gtsam::Pose3,
+               gtsam::Similarity2,
+               gtsam::Similarity3,
                gtsam::Unit3,
                gtsam::Cal3_S2,
                gtsam::Cal3DS2,
@@ -633,7 +639,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
 #include <gtsam/nonlinear/NonlinearEquality.h>
 template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
                gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
-               gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
+               gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
                gtsam::PinholeCamera<gtsam::Cal3_S2>,
                gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
@@ -651,7 +657,7 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
 
 template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
                gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
-               gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
+               gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
                gtsam::PinholeCamera<gtsam::Cal3_S2>,
                gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
@@ -720,7 +726,7 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
   gtsam::NonlinearFactorGraph getFactors() const;
 
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
-                     gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
+                     gtsam::Rot3, gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Cal3_S2, gtsam::Cal3DS2,
                      gtsam::Vector, gtsam::Matrix}>
   VALUE calculateEstimate(size_t key) const;
 };
@@ -732,6 +738,8 @@ template <T = {gtsam::Point2,
                gtsam::Rot3,
                gtsam::Pose2,
                gtsam::Pose3,
+               gtsam::Similarity2,
+               gtsam::Similarity3,
                gtsam::NavState,
                gtsam::imuBias::ConstantBias}>
 virtual class ExtendedKalmanFilter {

gtsam/nonlinear/values.i

@@ -17,6 +17,8 @@ namespace gtsam {
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Similarity2.h>
+#include <gtsam/geometry/Similarity3.h>
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/SO3.h>
@@ -80,6 +82,8 @@ class Values {
   void insert(size_t j, const gtsam::SOn& P);
   void insert(size_t j, const gtsam::Rot3& rot3);
   void insert(size_t j, const gtsam::Pose3& pose3);
+  void insert(size_t j, const gtsam::Similarity2& similarity2);
+  void insert(size_t j, const gtsam::Similarity3& similarity3);
   void insert(size_t j, const gtsam::Unit3& unit3);
   void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
   void insert(size_t j, const gtsam::Cal3f& cal3f);
@@ -122,6 +126,8 @@ class Values {
   void update(size_t j, const gtsam::SOn& P);
   void update(size_t j, const gtsam::Rot3& rot3);
   void update(size_t j, const gtsam::Pose3& pose3);
+  void update(size_t j, const gtsam::Similarity2& similarity2);
+  void update(size_t j, const gtsam::Similarity3& similarity3);
   void update(size_t j, const gtsam::Unit3& unit3);
   void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
   void update(size_t j, const gtsam::Cal3f& cal3f);
@@ -161,6 +167,8 @@ class Values {
   void insert_or_assign(size_t j, const gtsam::SOn& P);
   void insert_or_assign(size_t j, const gtsam::Rot3& rot3);
   void insert_or_assign(size_t j, const gtsam::Pose3& pose3);
+  void insert_or_assign(size_t j, const gtsam::Similarity2& similarity2);
+  void insert_or_assign(size_t j, const gtsam::Similarity3& similarity3);
   void insert_or_assign(size_t j, const gtsam::Unit3& unit3);
   void insert_or_assign(size_t j, const gtsam::Cal3Bundler& cal3bundler);
   void insert_or_assign(size_t j, const gtsam::Cal3f& cal3f);
@@ -196,6 +204,8 @@ class Values {
                  gtsam::SOn,
                  gtsam::Rot3,
                  gtsam::Pose3,
+                 gtsam::Similarity2,
+                 gtsam::Similarity3,
                  gtsam::Unit3,
                  gtsam::Cal3Bundler,
                  gtsam::Cal3f,