Merge pull request #334 from martinvl/fix/unit3-copy-assign

Fix Unit3 copy assign

gtsam/geometry/Unit3.h

@@ -90,6 +90,8 @@ public:
   /// Copy assignment
   Unit3& operator=(const Unit3 & u) {
     p_ = u.p_;
+    B_ = u.B_;
+    H_B_ = u.H_B_;
     return *this;
   }
 

gtsam/geometry/tests/testUnit3.cpp

@@ -484,6 +484,15 @@ TEST(Unit3, ErrorBetweenFactor) {
   }
 }
 
+TEST(Unit3, CopyAssign) {
+  Unit3 p{1, 0.2, 0.3};
+
+  EXPECT(p.error(p).isZero());
+
+  p = Unit3{-1, 2, 8};
+  EXPECT(p.error(p).isZero());
+}
+
 /* ************************************************************************* */
 TEST(actualH, Serialization) {
   Unit3 p(0, 1, 0);