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

included priors and robust model in example

release/4.3a0
Luca 2014-05-16 20:50:06 -04:00
parent 0fad251355
commit 5a6d719690
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
examples/Pose2SLAMExampleRobust_g2o.cpp
View File

@ -85,12 +85,17 @@ int main(const int argc, const char *argv[]){
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances((Vector(3) << 1/I11, 1/I22, 1/I33)); noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances((Vector(3) << 1/I11, 1/I22, 1/I33));
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2, model)); NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), model)));
graph.add(factor); graph.add(factor);
} }
is.ignore(LINESIZE, '\n'); is.ignore(LINESIZE, '\n');
} }
// graph.print("graph");
// otherwise GTSAM cannot solve the problem
NonlinearFactorGraph graphWithPrior = graph;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 0.01, 0.01, 0.001));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
// GaussNewtonParams parameters; // GaussNewtonParams parameters;
// Stop iterating once the change in error between steps is less than this value // Stop iterating once the change in error between steps is less than this value
@ -99,7 +104,7 @@ int main(const int argc, const char *argv[]){
// parameters.maxIterations = 100; // parameters.maxIterations = 100;
// Create the optimizer ... // Create the optimizer ...
std::cout << "Optimizing the factor graph" << std::endl; std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonOptimizer optimizer(graph, initial); // , parameters); GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters);
// ... and optimize // ... and optimize
Values result = optimizer.optimize(); Values result = optimizer.optimize();
// result.print("results"); // result.print("results");