From c167430389ed0b9ac6fa3a9e872d23d83b2f2fe6 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 19 May 2014 10:53:59 -0400 Subject: [PATCH] included Tukey --- examples/Pose2SLAMExampleRobust_g2o.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/examples/Pose2SLAMExampleRobust_g2o.cpp b/examples/Pose2SLAMExampleRobust_g2o.cpp index 94a19f7b2..e326838f2 100644 --- a/examples/Pose2SLAMExampleRobust_g2o.cpp +++ b/examples/Pose2SLAMExampleRobust_g2o.cpp @@ -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)); + // NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, l1Xl2, + // noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model))); + NonlinearFactor::shared_ptr factor(new BetweenFactor(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); } is.ignore(LINESIZE, '\n'); } + //std::cout << "Robust kernel: Huber" << std::endl; + std::cout << "Robust kernel: Tukey" << std::endl; + // 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));