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

--update the distance() of robust noise model 

--add a nonlinear optimization unit test using robust noise model
release/4.3a0
Yong-Dian Jian 2012-11-06 22:42:01 +00:00
parent 43f08ce806
commit 43778a6ed3
2 changed files with 28 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/linear/NoiseModel.h
View File

@ -699,7 +699,7 @@ namespace gtsam {
inline virtual Vector unwhiten(const Vector& v) const
{ throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
inline virtual double distance(const Vector& v) const
{ throw std::invalid_argument("distance is not currently supported for robust noise models."); }
{ return this->whiten(v).squaredNorm(); }
// TODO: these are really robust iterated re-weighting support functions
virtual void WhitenSystem(Vector& b) const;

27
tests/testNonlinearOptimizer.cpp
View File

@ -255,6 +255,33 @@ TEST(NonlinearOptimizer, MoreOptimization) {
EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize()));
}
/* ************************************************************************* */
TEST(NonlinearOptimizer, MoreOptimizationWithHuber) {
NonlinearFactorGraph fg;
fg.add(PriorFactor<Pose2>(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)));
fg.add(BetweenFactor<Pose2>(0, 1, Pose2(1,0,M_PI/2),
noiseModel::Robust::Create(noiseModel::MEstimator::Huber::Create(2.0),
noiseModel::Isotropic::Sigma(3,1))));
fg.add(BetweenFactor<Pose2>(1, 2, Pose2(1,0,M_PI/2),
noiseModel::Robust::Create(noiseModel::MEstimator::Huber::Create(3.0),
noiseModel::Isotropic::Sigma(3,1))));
Values init;
init.insert(0, Pose2(10,10,0));
init.insert(1, Pose2(1,0,M_PI));
init.insert(2, Pose2(1,1,-M_PI));
Values expected;
expected.insert(0, Pose2(0,0,0));
expected.insert(1, Pose2(1,0,M_PI/2));
expected.insert(2, Pose2(1,1,M_PI));
EXPECT(assert_equal(expected, GaussNewtonOptimizer(fg, init).optimize()));
EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init).optimize()));
EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize()));
}
/* ************************************************************************* */
int main() {
TestResult tr;