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

included Tukey

release/4.3a0
Luca 2014-05-19 10:53:59 -04:00
parent 5a6d719690
commit c167430389
1 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
examples/Pose2SLAMExampleRobust_g2o.cpp
View File

@ -85,13 +85,20 @@ 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,
// noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model)));
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2, NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), model))); noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model)));
graph.add(factor); graph.add(factor);
} }
is.ignore(LINESIZE, '\n'); is.ignore(LINESIZE, '\n');
} }
//std::cout << "Robust kernel: Huber" << std::endl;
std::cout << "Robust kernel: Tukey" << std::endl;
// otherwise GTSAM cannot solve the problem // otherwise GTSAM cannot solve the problem
NonlinearFactorGraph graphWithPrior = graph; NonlinearFactorGraph graphWithPrior = graph;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 0.01, 0.01, 0.001)); noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 0.01, 0.01, 0.001));