12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix serialization of AttitudeFactor and add roundtrip test

release/4.3a0
cbeall 2018-11-15 11:06:32 -08:00 committed by Chris Beall
parent bf1edde6e2
commit e5cd8c6dae
2 changed files with 45 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/navigation/AttitudeFactor.h
View File

@ -55,6 +55,14 @@ public:
/** vector of errors */ /** vector of errors */
Vector attitudeError(const Rot3& p, Vector attitudeError(const Rot3& p,
OptionalJacobian<2,3> H = boost::none) const; OptionalJacobian<2,3> H = boost::none) const;
/** 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_));
}
}; };
/** /**
@ -123,11 +131,10 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar ar & boost::serialization::make_nvp("NoiseModelFactor1",
& boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
boost::serialization::base_object<Base>(*this)); ar & boost::serialization::make_nvp("AttitudeFactor",
ar & BOOST_SERIALIZATION_NVP(nZ_); boost::serialization::base_object<AttitudeFactor>(*this));
ar & BOOST_SERIALIZATION_NVP(bRef_);
} }
public: public:
@ -210,11 +217,10 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar ar & boost::serialization::make_nvp("NoiseModelFactor1",
& boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
boost::serialization::base_object<Base>(*this)); ar & boost::serialization::make_nvp("AttitudeFactor",
ar & BOOST_SERIALIZATION_NVP(nZ_); boost::serialization::base_object<AttitudeFactor>(*this));
ar & BOOST_SERIALIZATION_NVP(bRef_);
} }
public: public:

29
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -19,6 +19,8 @@
#include <gtsam/navigation/AttitudeFactor.h> #include <gtsam/navigation/AttitudeFactor.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serialization.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace std; using namespace std;
@ -57,6 +59,22 @@ TEST( Rot3AttitudeFactor, Constructor ) {
EXPECT(assert_equal(expectedH, actualH, 1e-8)); EXPECT(assert_equal(expectedH, actualH, 1e-8));
} }
/* ************************************************************************* */
// Export Noisemodels
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
/* ************************************************************************* */
TEST(Rot3AttitudeFactor, Serialization) {
Unit3 nDown(0, 0, -1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
Rot3AttitudeFactor factor(0, nDown, model);
EXPECT(serializationTestHelpers::equalsObj(factor));
EXPECT(serializationTestHelpers::equalsXML(factor));
EXPECT(serializationTestHelpers::equalsBinary(factor));
}
// ************************************************************************* // *************************************************************************
TEST( Pose3AttitudeFactor, Constructor ) { TEST( Pose3AttitudeFactor, Constructor ) {
@ -90,6 +108,17 @@ TEST( Pose3AttitudeFactor, Constructor ) {
EXPECT(assert_equal(expectedH, actualH, 1e-8)); EXPECT(assert_equal(expectedH, actualH, 1e-8));
} }
/* ************************************************************************* */
TEST(Pose3AttitudeFactor, Serialization) {
Unit3 nDown(0, 0, -1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
Pose3AttitudeFactor factor(0, nDown, model);
EXPECT(serializationTestHelpers::equalsObj(factor));
EXPECT(serializationTestHelpers::equalsXML(factor));
EXPECT(serializationTestHelpers::equalsBinary(factor));
}
// ************************************************************************* // *************************************************************************
int main() { int main() {
TestResult tr; TestResult tr;