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

Now serializes noise model

release/4.3a0
Frank Dellaert 2015-07-11 13:52:27 -07:00
parent b52f488d75
commit 07bb930dbb
2 changed files with 21 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/sam/BearingFactor.h
View File

@ -56,10 +56,10 @@ class BearingFactor : public ExpressionFactor<Measured> {
GTSAM_CONCEPT_TESTABLE_TYPE(Measured)
GTSAM_CONCEPT_POSE_TYPE(Pose)
public:
/// Default constructor
BearingFactor() {}
public:
/// primary constructor
BearingFactor(Key poseKey, Key pointKey, const Measured& measured,
const SharedNoiseModel& model)
@ -83,9 +83,9 @@ class BearingFactor : public ExpressionFactor<Measured> {
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Factor", boost::serialization::base_object<Factor>(*this));
// ar& BOOST_SERIALIZATION_NVP(this->noiseModel_);
// ar& BOOST_SERIALIZATION_NVP(measurement_);
"NoiseModelFactor",
boost::serialization::base_object<NoiseModelFactor>(*this));
ar& boost::serialization::make_nvp("measurement_", this->measurement_);
}
template <class Archive>
@ -94,4 +94,9 @@ class BearingFactor : public ExpressionFactor<Measured> {
}; // BearingFactor
/// traits
template <class Pose, class Point, class Measured>
struct traits<BearingFactor<Pose, Point, Measured> >
: public Testable<BearingFactor<Pose, Point, Measured> > {};
} // namespace gtsam

14
gtsam/sam/tests/testBearingFactor.cpp
View File

@ -21,6 +21,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/base/serialization.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
@ -31,19 +32,28 @@ using namespace gtsam;
static SharedNoiseModel model(noiseModel::Unit::Create(1));
typedef BearingFactor<Pose2, Point2> BearingFactor2D;
typedef BearingFactor<Pose3, Point3> BearingFactor3D;
typedef BearingFactor<Pose3, Point3, Unit3> BearingFactor3D;
Key poseKey(1);
Key pointKey(2);
GTSAM_CONCEPT_TESTABLE_INST(BearingFactor2D)
/* ************************************************************************* */
// Export Noisemodels
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
/* ************************************************************************* */
TEST(BearingFactor, 2D) {
// Create a factor
double measurement(10.0);
BearingFactor<Pose2, Point2> factor(poseKey, pointKey, measurement, model);
BearingFactor2D factor(poseKey, pointKey, measurement, model);
std::string serialized = serializeXML(factor);
cout << serialized << endl;
EXPECT(serializationTestHelpers::equalsObj<BearingFactor2D>(factor));
// Set the linearization point
Values values;
values.insert(poseKey, Pose2(1.0, 2.0, 0.57));