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

Fixed SolverComparer for unordered

release/4.3a0
Richard Roberts 2013-08-01 21:57:55 +00:00
parent a3ea813518
commit af8f302402
1 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
examples/SolverComparer.cpp
View File

@ -26,6 +26,7 @@
#include <gtsam/nonlinear/Marginals.h> #include <gtsam/nonlinear/Marginals.h>
#include <fstream> #include <fstream>
#include <iostream>
#include <boost/archive/binary_oarchive.hpp> #include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp> #include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/export.hpp> #include <boost/serialization/export.hpp>
@ -188,6 +189,7 @@ int main(int argc, char *argv[]) {
/* ************************************************************************* */ /* ************************************************************************* */
void runIncremental() void runIncremental()
{ {
#if 0
ISAM2 isam2; ISAM2 isam2;
// Look for the first measurement to use // Look for the first measurement to use
@ -383,6 +385,7 @@ void runIncremental()
// cout << e.what() << endl; // cout << e.what() << endl;
//} //}
//tictoc_print_(); //tictoc_print_();
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -394,7 +397,9 @@ void runBatch()
measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(Pose::Dim()))); measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(Pose::Dim())));
gttic_(Create_optimizer); gttic_(Create_optimizer);
GaussNewtonOptimizer optimizer(measurements, initial); GaussNewtonParams params;
params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_QR;
GaussNewtonOptimizer optimizer(measurements, initial, params);
gttoc_(Create_optimizer); gttoc_(Create_optimizer);
double lastError; double lastError;
do { do {
@ -477,15 +482,14 @@ void runPerturb()
// Perturb values // Perturb values
VectorValues noise; VectorValues noise;
Ordering ordering = *initial.orderingArbitrary();
BOOST_FOREACH(const Values::KeyValuePair& key_val, initial) BOOST_FOREACH(const Values::KeyValuePair& key_val, initial)
{ {
Vector noisev(key_val.value.dim()); Vector noisev(key_val.value.dim());
for(Vector::Index i = 0; i < noisev.size(); ++i) for(Vector::Index i = 0; i < noisev.size(); ++i)
noisev(i) = normal(rng); noisev(i) = normal(rng);
noise.insert(ordering[key_val.key], noisev); noise.insert(key_val.key, noisev);
} }
Values perturbed = initial.retract(noise, ordering); Values perturbed = initial.retract(noise);
// Write results // Write results
try { try {