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

Merge pull request #816 from ScottMcMichael/fix/isam2-serialize 

Fix serialization of ISAM2 class
release/4.3a0
Fan Jiang 2021-07-19 10:44:51 -04:00 committed by GitHub
parent fda659674e 1d75a0738a
commit 4cd3eae170
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 110 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/inference/VariableIndex.h
View File

@ -182,6 +182,16 @@ protected:
return item->second;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(index_);
ar & BOOST_SERIALIZATION_NVP(nFactors_);
ar & BOOST_SERIALIZATION_NVP(nEntries_);
}
/// @}
};

20
gtsam/nonlinear/ISAM2.h
View File

@ -315,6 +315,26 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
void removeVariables(const KeySet& unusedKeys);
void updateDelta(bool forceFullSolve = false) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::base_object<BayesTree<ISAM2Clique> >(*this);
ar & BOOST_SERIALIZATION_NVP(theta_);
ar & BOOST_SERIALIZATION_NVP(variableIndex_);
ar & BOOST_SERIALIZATION_NVP(delta_);
ar & BOOST_SERIALIZATION_NVP(deltaNewton_);
ar & BOOST_SERIALIZATION_NVP(RgProd_);
ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_);
ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_);
ar & BOOST_SERIALIZATION_NVP(linearFactors_);
ar & BOOST_SERIALIZATION_NVP(doglegDelta_);
ar & BOOST_SERIALIZATION_NVP(fixedVariables_);
ar & BOOST_SERIALIZATION_NVP(update_count_);
}
}; // ISAM2
/// traits

80
gtsam/nonlinear/tests/testSerializationNonlinear.cpp
View File

@ -17,6 +17,7 @@
*/
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
@ -31,6 +32,30 @@ using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */
// Create GUIDs for Noisemodels
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
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::noiseModel::Robust, "gtsam_noiseModel_Robust");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* ************************************************************************* */
// Create GUIDs for factors
BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor<gtsam::Pose3>, "gtsam::PriorFactor<gtsam::Pose3>");
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional");
/* ************************************************************************* */
// Export all classes derived from Value
GTSAM_VALUE_EXPORT(gtsam::Cal3_S2);
@ -82,6 +107,61 @@ TEST (Serialization, TemplatedValues) {
EXPECT(equalsBinary(values));
}
TEST(Serialization, ISAM2) {
gtsam::ISAM2Params parameters;
gtsam::ISAM2 solver(parameters);
gtsam::NonlinearFactorGraph graph;
gtsam::Values initialValues;
initialValues.clear();
gtsam::Vector6 temp6;
for (int i = 0; i < 6; ++i) {
temp6[i] = 0.0001;
}
gtsam::noiseModel::Diagonal::shared_ptr noiseModel = gtsam::noiseModel::Diagonal::Sigmas(temp6);
gtsam::Pose3 pose0(gtsam::Rot3(), gtsam::Point3(0, 0, 0));
gtsam::Symbol symbol0('x', 0);
graph.add(gtsam::PriorFactor<gtsam::Pose3>(symbol0, pose0, noiseModel));
initialValues.insert(symbol0, pose0);
solver.update(graph, initialValues,
gtsam::FastVector<gtsam::FactorIndex>());
std::string binaryPath = "saved_solver.dat";
try {
std::ofstream outputStream(binaryPath);
boost::archive::binary_oarchive outputArchive(outputStream);
outputArchive << solver;
} catch(...) {
EXPECT(false);
}
gtsam::ISAM2 solverFromDisk;
try {
std::ifstream ifs(binaryPath);
boost::archive::binary_iarchive inputArchive(ifs);
inputArchive >> solverFromDisk;
} catch(...) {
EXPECT(false);
}
gtsam::Pose3 p1, p2;
try {
p1 = solver.calculateEstimate<gtsam::Pose3>(symbol0);
} catch(std::exception &e) {
EXPECT(false);
}
try {
p2 = solverFromDisk.calculateEstimate<gtsam::Pose3>(symbol0);
} catch(std::exception &e) {
EXPECT(false);
}
EXPECT(assert_equal(p1, p2));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */