diff --git a/doc/robust.pdf b/doc/robust.pdf index 3404719b4..ef8dc6fe6 100644 Binary files a/doc/robust.pdf and b/doc/robust.pdf differ diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 206cab3b1..a7b48b034 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -661,11 +661,6 @@ Robust::shared_ptr Robust::Create(const RobustModel::shared_ptr& robust, return shared_ptr(new Robust(robust, gaussian)); } -// Robust::shared_ptr Robust::Create( -// const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){ -// return shared_ptr(new Robust(robust,noise)); -// } - /* ************************************************************************* */ } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index c8e0e78a5..73484799f 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -94,7 +94,9 @@ namespace gtsam { virtual double error(const Vector& v) const = 0; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - virtual double distance(const Vector& v) const = 0; + virtual double distance(const Vector& v) { + return error(v) * 2; + } #endif virtual void WhitenSystem(std::vector& A, Vector& b) const = 0; @@ -713,6 +715,11 @@ namespace gtsam { { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } inline virtual Vector unwhiten(const Vector& /*v*/) const { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + inline virtual double distance(const Vector& v) { + return robust_->loss(this->unweightedWhiten(v).norm()); + } +#endif // Fold the use of the m-estimator loss(...) function into error(...) inline virtual double error(const Vector& v) const { return robust_->loss(noise_->mahalanobisDistance(v)); } @@ -734,9 +741,6 @@ namespace gtsam { static shared_ptr Create( const RobustModel::shared_ptr &robust, const noiseModel::Base::shared_ptr noise); - // static shared_ptr Create( - // const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise); - private: /** Serialization function */ friend class boost::serialization::access;