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

Added ISAM2 serialize test

release/4.3a0
Scott 2021-07-09 18:30:39 -07:00
parent df579ec6a7
commit 8c68e21521
1 changed files with 79 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
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,60 @@ 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<size_t>());
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); }
/* ************************************************************************* */