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

Merged in fix/SmartFactorSerialization (pull request #202) 

fixed smart factors / IMU factor serialization, add unit tests
release/4.3a0
Frank Dellaert 2016-01-14 07:58:22 -08:00
parent 31eec5317c c3edee1e2d
commit 25948677c5
12 changed files with 169 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/geometry/triangulation.h
View File

@ -376,6 +376,15 @@ class TriangulationResult: public boost::optional<Point3> {
status_(s) {
}
public:
/**
* Default constructor, only for serialization
*/
TriangulationResult() {}
/**
* Constructor
*/
TriangulationResult(const Point3& p) :
status_(VALID) {
reset(p);

1
gtsam/navigation/ImuBias.h
View File

@ -201,7 +201,6 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("imuBias::ConstantBias", *this);
ar & BOOST_SERIALIZATION_NVP(biasAcc_);
ar & BOOST_SERIALIZATION_NVP(biasGyro_);
}

8
gtsam/navigation/ImuFactor.h
View File

@ -128,8 +128,9 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size()));
}
};
@ -167,7 +168,7 @@ public:
#endif
/** Default constructor - only use for serialization */
ImuFactor();
ImuFactor() {}
/**
* Constructor
@ -241,4 +242,7 @@ private:
};
// class ImuFactor
/// traits
template<> struct traits<ImuFactor> : public Testable<ImuFactor> {};
} /// namespace gtsam

3
gtsam/navigation/PreintegratedRotation.h
View File

@ -52,7 +52,8 @@ public:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
namespace bs = ::boost::serialization;
ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
}

21
gtsam/navigation/PreintegrationBase.h
View File

@ -84,15 +84,17 @@ public:
protected:
/// Default constructor for serialization only: uninitialized!
Params();
Params() {}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
namespace bs = ::boost::serialization;
ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
boost::serialization::base_object<PreintegratedRotation::Params>(*this));
ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
ar & BOOST_SERIALIZATION_NVP(n_gravity);
}
@ -246,15 +248,16 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
}
};

46
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -1010,6 +1010,52 @@ TEST(ImuFactor, bodyPSensorWithBias) {
EXPECT(assert_equal(biasExpected, biasActual, 1e-3));
}
/* ************************************************************************** */
#include <gtsam/base/serializationTestHelpers.h>
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST(ImuFactor, serialization) {
using namespace gtsam::serializationTestHelpers;
Vector3 n_gravity(0, 0, -9.81);
Pose3 body_P_sensor(Rot3::ypr(0, 0, M_PI), Point3());
Matrix3 accCov = 1e-7 * I_3x3;
Matrix3 gyroCov = 1e-8 * I_3x3;
Matrix3 integrationCov = 1e-9 * I_3x3;
double deltaT = 0.005;
imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
ImuFactor::PreintegratedMeasurements pim =
ImuFactor::PreintegratedMeasurements(priorBias, accCov, gyroCov,
integrationCov, true);
// measurements are needed for non-inf noise model, otherwise will throw err when deserialize
// Sensor frame is z-down
// Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame
Vector3 measuredOmega(0, 0.01, 0);
// Acc measurement is acceleration of sensor in the sensor frame, when stationary,
// table exerts an equal and opposite force w.r.t gravity
Vector3 measuredAcc(0, 0, -9.81);
for (int j = 0; j < 200; ++j)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT,
body_P_sensor);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

3
gtsam/slam/SmartFactorBase.h
View File

@ -93,6 +93,9 @@ public:
/// We use the new CameraSte data structure to refer to a set of cameras
typedef CameraSet<CAMERA> Cameras;
/// Default Constructor, for serialization
SmartFactorBase() {}
/// Constructor
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none) :

7
gtsam/slam/SmartProjectionFactor.h
View File

@ -137,7 +137,7 @@ protected:
/// @name Parameters
/// @{
const SmartProjectionParams params_;
SmartProjectionParams params_;
/// @}
/// @name Caching triangulation
@ -154,6 +154,11 @@ public:
/// shorthand for a set of cameras
typedef CameraSet<CAMERA> Cameras;
/**
* Default constructor, only for serialization
*/
SmartProjectionFactor() {}
/**
* Constructor
* @param body_P_sensor pose of the camera in the body frame

5
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -59,6 +59,11 @@ public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/**
* Default constructor, only for serialization
*/
SmartProjectionPoseFactor() {}
/**
* Constructor
* @param Isotropic measurement noise

37
gtsam/slam/tests/testSmartFactorBase.cpp
View File

@ -18,6 +18,7 @@
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
@ -29,9 +30,13 @@ static SharedNoiseModel unit3(noiseModel::Unit::Create(3));
/* ************************************************************************* */
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
namespace gtsam {
class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
public:
typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
PinholeFactor() {}
PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
}
virtual double error(const Values& values) const {
@ -43,6 +48,11 @@ public:
}
};
/// traits
template<>
struct traits<PinholeFactor> : public Testable<PinholeFactor> {};
}
TEST(SmartFactorBase, Pinhole) {
PinholeFactor f= PinholeFactor(unit2);
f.add(Point2(), 1);
@ -52,9 +62,13 @@ TEST(SmartFactorBase, Pinhole) {
/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>
namespace gtsam {
class StereoFactor: public SmartFactorBase<StereoCamera> {
public:
typedef SmartFactorBase<StereoCamera> Base;
StereoFactor() {}
StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
}
virtual double error(const Values& values) const {
@ -66,6 +80,11 @@ public:
}
};
/// traits
template<>
struct traits<StereoFactor> : public Testable<StereoFactor> {};
}
TEST(SmartFactorBase, Stereo) {
StereoFactor f(unit3);
f.add(StereoPoint2(), 1);
@ -73,6 +92,24 @@ TEST(SmartFactorBase, Stereo) {
EXPECT_LONGS_EQUAL(2 * 3, f.dim());
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST(SmartFactorBase, serialize) {
using namespace gtsam::serializationTestHelpers;
PinholeFactor factor(unit2);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

21
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -22,6 +22,7 @@
#include "smartFactorScenarios.h"
#include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <iostream>
@ -843,6 +844,26 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
EXPECT(assert_equal(yActual, yExpected, 1e-7));
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST( SmartProjectionCameraFactor, serialize) {
using namespace vanilla;
using namespace gtsam::serializationTestHelpers;
SmartFactor factor(unit2);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

22
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -24,6 +24,7 @@
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <iostream>
@ -1387,6 +1388,27 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
values.at<Pose3>(x3)));
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST(SmartProjectionPoseFactor, serialize) {
using namespace vanillaPose;
using namespace gtsam::serializationTestHelpers;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
SmartFactor factor(model, sharedK, boost::none, params);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;