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

Added (de)serialization to UnorderedLinear timing example so that it loads faster

release/4.3a0
Richard Roberts 2013-07-23 18:34:24 +00:00
parent c788c525ea
commit 6b1e1d16d5
1 changed files with 65 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
examples/UnorderedLinear.cpp
View File

@ -4,6 +4,7 @@
#include <gtsam/linear/GaussianFactorGraphUnordered.h> #include <gtsam/linear/GaussianFactorGraphUnordered.h>
#include <gtsam/linear/VectorValuesUnordered.h> #include <gtsam/linear/VectorValuesUnordered.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h> #include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/linear/GaussianBayesTreeUnordered.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Symbol.h> #include <gtsam/nonlinear/Symbol.h>
@ -11,10 +12,28 @@
#include <string> #include <string>
#include <boost/timer/timer.hpp> #include <boost/timer/timer.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/export.hpp>
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
BOOST_CLASS_EXPORT(Value);
BOOST_CLASS_EXPORT(Pose2);
BOOST_CLASS_EXPORT(NonlinearFactor);
BOOST_CLASS_EXPORT(NoiseModelFactor);
BOOST_CLASS_EXPORT(NoiseModelFactor1<Pose2>);
typedef NoiseModelFactor2<Pose2,Pose2> NMF2;
BOOST_CLASS_EXPORT(NMF2);
BOOST_CLASS_EXPORT(BetweenFactor<Pose2>);
BOOST_CLASS_EXPORT(PriorFactor<Pose2>);
BOOST_CLASS_EXPORT(noiseModel::Base);
BOOST_CLASS_EXPORT(noiseModel::Isotropic);
BOOST_CLASS_EXPORT(noiseModel::Gaussian);
BOOST_CLASS_EXPORT(noiseModel::Diagonal);
BOOST_CLASS_EXPORT(noiseModel::Unit);
double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
// Compute degrees of freedom (observations - variables) // Compute degrees of freedom (observations - variables)
// In ocaml, +1 was added to the observations to account for the prior, but // In ocaml, +1 was added to the observations to account for the prior, but
@ -22,9 +41,9 @@ double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& c
// double dof = graph.size() - config.size(); // double dof = graph.size() - config.size();
int graph_dim = 0; int graph_dim = 0;
BOOST_FOREACH(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf, graph) { BOOST_FOREACH(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf, graph) {
graph_dim += nlf->dim(); graph_dim += (int)nlf->dim();
} }
double dof = graph_dim - config.dim(); // kaess: changed to dim double dof = double(graph_dim - config.dim()); // kaess: changed to dim
return 2. * graph.error(config) / dof; // kaess: added factor 2, graph.error returns half of actual error return 2. * graph.error(config) / dof; // kaess: added factor 2, graph.error returns half of actual error
} }
@ -34,7 +53,7 @@ ISAM2 solveWithOldISAM2(const NonlinearFactorGraph& measurements)
ISAM2 isam2; ISAM2 isam2;
size_t nextMeasurement = 0; size_t nextMeasurement = 0;
for(size_t step=1; nextMeasurement < measurements.size(); ++step) { for(size_t step=1; nextMeasurement < measurements.size() && nextMeasurement < 2000; ++step) {
Values newVariables; Values newVariables;
NonlinearFactorGraph newFactors; NonlinearFactorGraph newFactors;
@ -161,23 +180,59 @@ int main(int argc, char *argv[])
// Solve with old ISAM2 to get initial estimate // Solve with old ISAM2 to get initial estimate
cout << "Solving with old ISAM2 to obtain initial estimate" << endl; cout << "Solving with old ISAM2 to obtain initial estimate" << endl;
gttic_(Old_ISAM2); gttic_(Old_ISAM2);
ISAM2 isam = solveWithOldISAM2(measurements);
Values isamsoln = isam.calculateEstimate(); //try {
// ISAM2 isam = solveWithOldISAM2(measurements);
// {
// std::ofstream writerStream("incremental_init2000", ios::binary);
// boost::archive::binary_oarchive writer(writerStream);
// writer << isam.calculateEstimate();
// writerStream.close();
// }
// {
// std::ofstream writerStream("incremental_graph2000", ios::binary);
// boost::archive::binary_oarchive writer(writerStream);
// writer << isam.getFactorsUnsafe();
// writerStream.close();
// }
//} catch(std::exception& e) {
// cout << e.what() << endl;
//}
Values isamsoln;
NonlinearFactorGraph nlfg;
{
std::ifstream readerStream("incremental_init", ios::binary);
boost::archive::binary_iarchive reader(readerStream);
reader >> isamsoln;
}
{
std::ifstream readerStream("incremental_graph", ios::binary);
boost::archive::binary_iarchive reader(readerStream);
reader >> nlfg;
}
gttoc_(Old_ISAM2); gttoc_(Old_ISAM2);
// Get linear graph // Get linear graph
cout << "Converting to unordered linear graph" << endl; cout << "Converting to unordered linear graph" << endl;
Ordering ordering = *isam.getFactorsUnsafe().orderingCOLAMD(isamsoln); Ordering ordering = *nlfg.orderingCOLAMD(isamsoln);
GaussianFactorGraph gfg = *isam.getFactorsUnsafe().linearize(isamsoln, ordering); GaussianFactorGraph gfg = *nlfg.linearize(isamsoln, ordering);
GaussianFactorGraphUnordered gfgu = convertToUnordered(gfg, ordering); GaussianFactorGraphUnordered gfgu = convertToUnordered(gfg, ordering);
OrderingUnordered orderingUnordered;
for(Index j = 0; j < ordering.size(); ++j)
orderingUnordered.push_back(ordering.key(j));
// Solve linear graph // Solve linear graph
cout << "Optimizing unordered graph" << endl; cout << "Optimizing unordered graph" << endl;
{ {
boost::timer::cpu_timer timer; boost::timer::cpu_timer timer;
gttic_(Solve_unordered); gttic_(Solve_unordered);
for(size_t i = 0; i < 20; ++i) for(size_t i = 0; i < 10; ++i) {
VectorValuesUnordered unorderedSoln = gfgu.optimize(); VectorValuesUnordered unorderedSoln = gfgu.eliminateMultifrontal(orderingUnordered)->optimize();
}
gttoc_(Solve_unordered); gttoc_(Solve_unordered);
timer.stop(); timer.stop();
boost::timer::cpu_times times = timer.elapsed(); boost::timer::cpu_times times = timer.elapsed();
@ -190,7 +245,7 @@ int main(int argc, char *argv[])
{ {
boost::timer::cpu_timer timer; boost::timer::cpu_timer timer;
gttic_(Solve_ordered); gttic_(Solve_ordered);
for(size_t i = 0; i < 20; ++i) for(size_t i = 0; i < 10; ++i)
VectorValues orderedSoln = *GaussianMultifrontalSolver(gfg, true).optimize(); VectorValues orderedSoln = *GaussianMultifrontalSolver(gfg, true).optimize();
gttoc_(Solve_ordered); gttoc_(Solve_ordered);
timer.stop(); timer.stop();