diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index b80e6e4d8..27d41a014 100644 --- a/gtsam/geometry/Unit3.h +++ b/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; } diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index a25ab25c7..ddf60a256 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/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);