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 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("nZ_", const_cast(nZ_)); - ar & boost::serialization::make_nvp("bRef_", const_cast(bRef_)); + ar & boost::serialization::make_nvp("nZ_", nZ_); + ar & boost::serialization::make_nvp("bRef_", bRef_); } }; @@ -118,12 +125,6 @@ public: boost::optional 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: diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 70b78c916..38f16f55f 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -75,6 +75,23 @@ 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. + EXPECT(std::is_copy_assignable::value); + Rot3AttitudeFactor factor_copied = factor; + EXPECT(assert_equal(factor, factor_copied)); + + // Move assignable. + EXPECT(std::is_move_assignable::value); + Rot3AttitudeFactor factor_moved = std::move(factor_copied); + EXPECT(assert_equal(factor, factor_moved)); +} + // ************************************************************************* TEST( Pose3AttitudeFactor, Constructor ) { @@ -119,6 +136,23 @@ 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. + EXPECT(std::is_copy_assignable::value); + Pose3AttitudeFactor factor_copied = factor; + EXPECT(assert_equal(factor, factor_copied)); + + // Move assignable. + EXPECT(std::is_move_assignable::value); + Pose3AttitudeFactor factor_moved = std::move(factor_copied); + EXPECT(assert_equal(factor, factor_moved)); +} + // ************************************************************************* int main() { TestResult tr;