From 9518346161c21715d407bac5ff553f054b6fc876 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 2 Jan 2022 14:59:28 -0500
Subject: [PATCH] Fix unstable c++ examples

---
 .../EquivInertialNavFactor_GlobalVel_NoBias.h | 10 ++---
 gtsam_unstable/slam/serialization.cpp         | 16 --------
 .../tests/testGaussMarkov1stOrderFactor.cpp   | 41 ++++++++++---------
 .../slam/tests/testSerialization.cpp          |  2 +-
 4 files changed, 27 insertions(+), 42 deletions(-)

diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
index 0e2aebd7f..b053b13f8 100644
--- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
+++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
@@ -372,15 +372,15 @@ public:
     Matrix Z_3x3 = Z_3x3;
     Matrix I_3x3 = I_3x3;
 
-    Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
-    Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
+    Matrix H_pos_pos = numericalDerivative11<Vector, Vector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
+    Matrix H_pos_vel = numericalDerivative11<Vector, Vector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
     Matrix H_pos_angles = Z_3x3;
 
-    Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0);
-    Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles);
+    Matrix H_vel_vel = numericalDerivative11<Vector, Vector>(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0);
+    Matrix H_vel_angles = numericalDerivative11<Vector, Vector>(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles);
     Matrix H_vel_pos = Z_3x3;
 
-    Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
+    Matrix H_angles_angles = numericalDerivative11<Vector, Vector>(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
     Matrix H_angles_pos = Z_3x3;
     Matrix H_angles_vel = Z_3x3;
 
diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp
index 88a94fd51..bed11e535 100644
--- a/gtsam_unstable/slam/serialization.cpp
+++ b/gtsam_unstable/slam/serialization.cpp
@@ -5,8 +5,6 @@
  * @author Alex Cunningham
  */
 
-#include <gtsam/base/deprecated/LieMatrix_Deprecated.h>
-#include <gtsam/base/deprecated/LieVector_Deprecated.h>
 #include <gtsam/slam/serialization.h>
 #include <gtsam/base/serialization.h>
 
@@ -31,8 +29,6 @@
 using namespace gtsam;
 
 // Creating as many permutations of factors as possible
-typedef PriorFactor<LieVector>            PriorFactorLieVector;
-typedef PriorFactor<LieMatrix>            PriorFactorLieMatrix;
 typedef PriorFactor<Point2>               PriorFactorPoint2;
 typedef PriorFactor<StereoPoint2>         PriorFactorStereoPoint2;
 typedef PriorFactor<Point3>               PriorFactorPoint3;
@@ -46,8 +42,6 @@ typedef PriorFactor<CalibratedCamera>     PriorFactorCalibratedCamera;
 typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2;
 typedef PriorFactor<StereoCamera>         PriorFactorStereoCamera;
 
-typedef BetweenFactor<LieVector>       BetweenFactorLieVector;
-typedef BetweenFactor<LieMatrix>       BetweenFactorLieMatrix;
 typedef BetweenFactor<Point2>          BetweenFactorPoint2;
 typedef BetweenFactor<Point3>          BetweenFactorPoint3;
 typedef BetweenFactor<Rot2>            BetweenFactorRot2;
@@ -55,8 +49,6 @@ typedef BetweenFactor<Rot3>            BetweenFactorRot3;
 typedef BetweenFactor<Pose2>           BetweenFactorPose2;
 typedef BetweenFactor<Pose3>           BetweenFactorPose3;
 
-typedef NonlinearEquality<LieVector>              NonlinearEqualityLieVector;
-typedef NonlinearEquality<LieMatrix>              NonlinearEqualityLieMatrix;
 typedef NonlinearEquality<Point2>                 NonlinearEqualityPoint2;
 typedef NonlinearEquality<StereoPoint2>           NonlinearEqualityStereoPoint2;
 typedef NonlinearEquality<Point3>                 NonlinearEqualityPoint3;
@@ -112,8 +104,6 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
 
 /* Create GUIDs for geometry */
 /* ************************************************************************* */
-GTSAM_VALUE_EXPORT(gtsam::LieVector);
-GTSAM_VALUE_EXPORT(gtsam::LieMatrix);
 GTSAM_VALUE_EXPORT(gtsam::Point2);
 GTSAM_VALUE_EXPORT(gtsam::StereoPoint2);
 GTSAM_VALUE_EXPORT(gtsam::Point3);
@@ -133,8 +123,6 @@ GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
 BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
 BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
 
-BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector");
-BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix");
 BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2");
 BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2");
 BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3");
@@ -147,8 +135,6 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2");
 BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera");
 BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera");
 
-BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector");
-BOOST_CLASS_EXPORT_GUID(BetweenFactorLieMatrix, "gtsam::BetweenFactorLieMatrix");
 BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2");
 BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3");
 BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2");
@@ -156,8 +142,6 @@ BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3");
 BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2");
 BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3");
 
-BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieVector, "gtsam::NonlinearEqualityLieVector");
-BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieMatrix, "gtsam::NonlinearEqualityLieMatrix");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2");
 BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3");
diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
index 8692cf584..ed4092c60 100644
--- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
+++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
@@ -16,22 +16,23 @@
  * @date    Jan 17, 2012
  */
 
-#include <gtsam_unstable/slam/GaussMarkov1stOrderFactor.h>
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/inference/Key.h>
-#include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/deprecated/LieVector.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam_unstable/slam/GaussMarkov1stOrderFactor.h>
 
 using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 
 //! Factors
-typedef GaussMarkov1stOrderFactor<LieVector> GaussMarkovFactor;
+typedef GaussMarkov1stOrderFactor<Vector3> GaussMarkovFactor;
 
 /* ************************************************************************* */
-LieVector predictionError(const LieVector& v1, const LieVector& v2, const GaussMarkovFactor factor) {
+Vector predictionError(const Vector& v1, const Vector& v2,
+                       const GaussMarkovFactor factor) {
   return factor.evaluateError(v1, v2);
 }
 
@@ -58,29 +59,29 @@ TEST( GaussMarkovFactor, error )
   Key x1(1);
   Key x2(2);
   double delta_t = 0.10;
-  Vector tau = Vector3(100.0, 150.0, 10.0);
+  Vector3 tau(100.0, 150.0, 10.0);
   SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0);
 
-  LieVector v1 = LieVector(Vector3(10.0, 12.0, 13.0));
-  LieVector v2 = LieVector(Vector3(10.0, 15.0, 14.0));
+  Vector3 v1(10.0, 12.0, 13.0);
+  Vector3 v2(10.0, 15.0, 14.0);
 
   // Create two nodes
   linPoint.insert(x1, v1);
   linPoint.insert(x2, v2);
 
   GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
-  Vector Err1( factor.evaluateError(v1, v2) );
+  Vector3 error1 = factor.evaluateError(v1, v2);
 
   // Manually calculate the error
-  Vector alpha(tau.size());
-  Vector alpha_v1(tau.size());
+  Vector3 alpha(tau.size());
+  Vector3 alpha_v1(tau.size());
   for(int i=0; i<tau.size(); i++){
     alpha(i) = exp(- 1/tau(i)*delta_t );
     alpha_v1(i) = alpha(i) * v1(i);
   }
-  Vector Err2( v2 - alpha_v1 );
+  Vector3 error2 = v2 - alpha_v1;
 
-  CHECK(assert_equal(Err1, Err2, 1e-9));
+  CHECK(assert_equal(error1, error2, 1e-8));
 }
 
 /* ************************************************************************* */
@@ -90,14 +91,14 @@ TEST (GaussMarkovFactor, jacobian ) {
   Key x1(1);
   Key x2(2);
   double delta_t = 0.10;
-  Vector tau = Vector3(100.0, 150.0, 10.0);
+  Vector3 tau(100.0, 150.0, 10.0);
   SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0);
 
   GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
 
   // Update the linearization point
-  LieVector v1_upd = LieVector(Vector3(0.5, -0.7, 0.3));
-  LieVector v2_upd = LieVector(Vector3(-0.7, 0.4, 0.9));
+  Vector3 v1_upd(0.5, -0.7, 0.3);
+  Vector3 v2_upd(-0.7, 0.4, 0.9);
 
   // Calculate the Jacobian matrix using the factor
   Matrix computed_H1, computed_H2;
@@ -115,8 +116,8 @@ TEST (GaussMarkovFactor, jacobian ) {
       v1_upd, v2_upd);
 
   // Verify they are equal for this choice of state
-  CHECK( assert_equal(numerical_H1, computed_H1, 1e-9));
-  CHECK( assert_equal(numerical_H2, computed_H2, 1e-9));
+  CHECK(assert_equal(numerical_H1, computed_H1, 1e-9));
+  CHECK(assert_equal(numerical_H2, computed_H2, 1e-9));
 }
 
 /* ************************************************************************* */
diff --git a/gtsam_unstable/slam/tests/testSerialization.cpp b/gtsam_unstable/slam/tests/testSerialization.cpp
index 792fd1133..e9157317e 100644
--- a/gtsam_unstable/slam/tests/testSerialization.cpp
+++ b/gtsam_unstable/slam/tests/testSerialization.cpp
@@ -10,7 +10,7 @@
 #include <CppUnitLite/TestHarness.h>
 #include <iostream>
 
-#include <gtsam/slam/serialization.h>
+#include <gtsam_unstable/slam/serialization.h>
 
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>