--update the distance() of robust noise model

--add a nonlinear optimization unit test using robust noise model

gtsam/linear/NoiseModel.h

@@ -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;

tests/testNonlinearOptimizer.cpp

@@ -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;