FIX: Robust loss error calculation

[ProfFan]

This fixes #1129

[ProfFan]

@yetongumich

TEST(NoiseModel, robustNoiseL2WithDeadZone)
{
  double dead_zone_size = 1.0;
  SharedNoiseModel robust = noiseModel::Robust::Create(
      noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size),
      Unit::Create(3));

  for (int i = 0; i < 5; i++) {
    Vector3 error = Vector3(i, 0, 0);
    DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i,
                  robust->squaredMahalanobisDistance(error), 1e-8);
  }
}

is failing

[dellaert]

CI fails

[dellaert]

@ProfFan CI stil fails :-)

[dellaert]

@ProfFan CI fails. Also, would like a better PR comment.

[varunagrawal]

@ProfFan @yetongumich can we land this soon? @raabuchanan and I need to use robust noise models for a project we're working on and I would like to see the kinks worked out. :)

[ProfFan]

@varunagrawal I worked on this for quite a long time trying to figure out why the unit test fails but ran out of time before internship, could you also give an eye on it? I can look at it tomorrow.

[yetongumich]

@varunagrawal I worked on this for quite a long time trying to figure out why the unit test fails but ran out of time before internship, could you also give an eye on it? I can look at it tomorrow.

I already figured out why the test failed, maybe we can discuss about how to fix it tomorrow?

[yetongumich]

Updated the previous failing unit test.

The previous test is actually testing the WhitenSystem function for robust noise, which was indirectly called through squaredMahalanobisDistance function.

According to the new factorization, WhitenSystem is no longer called for robust noisemodel, therefore this test shall explicitly call WhitenSystem function to get the correct results.

[ProfFan]

Thanks @yetongumich ! I'll merge this @varunagrawal