From 660234e4ad7790d6e32c177c06b968c247d18ac6 Mon Sep 17 00:00:00 2001
From: Erik Nelson <eanelson@eecs.berkeley.edu>
Date: Fri, 22 Nov 2019 13:39:09 -0800
Subject: [PATCH 1/3] Make attitude factor copy and move assignable

---
 gtsam/navigation/AttitudeFactor.h | 25 ++++++++++---------------
 1 file changed, 10 insertions(+), 15 deletions(-)

diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h
index 19c1f8139..db588008e 100644
--- a/gtsam/navigation/AttitudeFactor.h
+++ b/gtsam/navigation/AttitudeFactor.h
@@ -35,7 +35,7 @@ class AttitudeFactor {
 
 protected:
 
-  const Unit3 nZ_, bRef_; ///< Position measurement in
+  Unit3 nZ_, bRef_; ///< Position measurement in
 
 public:
 
@@ -56,12 +56,19 @@ public:
   Vector attitudeError(const Rot3& p,
       OptionalJacobian<2,3> H = boost::none) const;
 
+  const Unit3& nZ() const {
+    return nZ_;
+  }
+  const Unit3& bRef() const {
+    return bRef_;
+  }
+
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-    ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
-    ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));
+    ar & boost::serialization::make_nvp("nZ_",  nZ_);
+    ar & boost::serialization::make_nvp("bRef_", bRef_);
   }
 };
 
@@ -118,12 +125,6 @@ public:
       boost::optional<Matrix&> H = boost::none) const {
     return attitudeError(nRb, H);
   }
-  Unit3 nZ() const {
-    return nZ_;
-  }
-  Unit3 bRef() const {
-    return bRef_;
-  }
 
 private:
 
@@ -204,12 +205,6 @@ public:
     }
     return e;
   }
-  Unit3 nZ() const {
-    return nZ_;
-  }
-  Unit3 bRef() const {
-    return bRef_;
-  }
 
 private:
 

From cb5de5be5813a42be89e1c4ee5ed42d6df70c6c5 Mon Sep 17 00:00:00 2001
From: Erik Nelson <eanelson@eecs.berkeley.edu>
Date: Fri, 22 Nov 2019 13:41:55 -0800
Subject: [PATCH 2/3] Add unit tests

---
 gtsam/navigation/tests/testAttitudeFactor.cpp | 30 +++++++++++++++++++
 1 file changed, 30 insertions(+)

diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp
index 70b78c916..24b7f2311 100644
--- a/gtsam/navigation/tests/testAttitudeFactor.cpp
+++ b/gtsam/navigation/tests/testAttitudeFactor.cpp
@@ -75,6 +75,21 @@ TEST(Rot3AttitudeFactor, Serialization) {
   EXPECT(serializationTestHelpers::equalsBinary(factor));
 }
 
+/* ************************************************************************* */
+TEST(Rot3AttitudeFactor, CopyAndMove) {
+  Unit3 nDown(0, 0, -1);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
+  Rot3AttitudeFactor factor(0, nDown, model);
+
+  // Copy assignable.
+  Rot3AttitudeFactor factor_copied = factor;
+  EXPECT(assert_equal(factor, factor_copied));
+
+  // Move assignable.
+  Rot3AttitudeFactor factor_moved = std::move(factor_copied);
+  EXPECT(assert_equal(factor, factor_moved));
+}
+
 // *************************************************************************
 TEST( Pose3AttitudeFactor, Constructor ) {
 
@@ -119,6 +134,21 @@ TEST(Pose3AttitudeFactor, Serialization) {
   EXPECT(serializationTestHelpers::equalsBinary(factor));
 }
 
+/* ************************************************************************* */
+TEST(Pose3AttitudeFactor, CopyAndMove) {
+  Unit3 nDown(0, 0, -1);
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
+  Pose3AttitudeFactor factor(0, nDown, model);
+
+  // Copy assignable.
+  Pose3AttitudeFactor factor_copied = factor;
+  EXPECT(assert_equal(factor, factor_copied));
+
+  // Move assignable.
+  Pose3AttitudeFactor factor_moved = std::move(factor_copied);
+  EXPECT(assert_equal(factor, factor_moved));
+}
+
 // *************************************************************************
 int main() {
   TestResult tr;

From 3f064894c4fdf80163f2945075acb51050b4782a Mon Sep 17 00:00:00 2001
From: Erik Nelson <eanelson@eecs.berkeley.edu>
Date: Fri, 22 Nov 2019 13:59:23 -0800
Subject: [PATCH 3/3] Explicitly check for copy and move assignable

---
 gtsam/navigation/tests/testAttitudeFactor.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp
index 24b7f2311..38f16f55f 100644
--- a/gtsam/navigation/tests/testAttitudeFactor.cpp
+++ b/gtsam/navigation/tests/testAttitudeFactor.cpp
@@ -82,10 +82,12 @@ TEST(Rot3AttitudeFactor, CopyAndMove) {
   Rot3AttitudeFactor factor(0, nDown, model);
 
   // Copy assignable.
+  EXPECT(std::is_copy_assignable<Rot3AttitudeFactor>::value);
   Rot3AttitudeFactor factor_copied = factor;
   EXPECT(assert_equal(factor, factor_copied));
 
   // Move assignable.
+  EXPECT(std::is_move_assignable<Rot3AttitudeFactor>::value);
   Rot3AttitudeFactor factor_moved = std::move(factor_copied);
   EXPECT(assert_equal(factor, factor_moved));
 }
@@ -141,10 +143,12 @@ TEST(Pose3AttitudeFactor, CopyAndMove) {
   Pose3AttitudeFactor factor(0, nDown, model);
 
   // Copy assignable.
+  EXPECT(std::is_copy_assignable<Pose3AttitudeFactor>::value);
   Pose3AttitudeFactor factor_copied = factor;
   EXPECT(assert_equal(factor, factor_copied));
 
   // Move assignable.
+  EXPECT(std::is_move_assignable<Pose3AttitudeFactor>::value);
   Pose3AttitudeFactor factor_moved = std::move(factor_copied);
   EXPECT(assert_equal(factor, factor_moved));
 }