implemented residual functions for common M-estimators and added corresponding tests
							parent
							
								
									3eb8e3d9bc
								
							
						
					
					
						commit
						8280a082bd
					
				
							
								
								
									
										6
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										6
									
								
								gtsam.h
								
								
								
								
							|  | @ -1364,6 +1364,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { | |||
|   void serializable() const; | ||||
| 
 | ||||
|   double weight(double error) const; | ||||
|   double residual(double error) const; | ||||
| }; | ||||
| 
 | ||||
| virtual class Fair: gtsam::noiseModel::mEstimator::Base { | ||||
|  | @ -1384,6 +1385,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { | |||
|   void serializable() const; | ||||
| 
 | ||||
|   double weight(double error) const; | ||||
|   double residual(double error) const; | ||||
| }; | ||||
| 
 | ||||
| virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { | ||||
|  | @ -1394,6 +1396,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { | |||
|   void serializable() const; | ||||
| 
 | ||||
|   double weight(double error) const; | ||||
|   double residual(double error) const; | ||||
| }; | ||||
| 
 | ||||
| virtual class Tukey: gtsam::noiseModel::mEstimator::Base { | ||||
|  | @ -1404,6 +1407,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { | |||
|   void serializable() const; | ||||
| 
 | ||||
|   double weight(double error) const; | ||||
|   double residual(double error) const; | ||||
| }; | ||||
| 
 | ||||
| virtual class Welsch: gtsam::noiseModel::mEstimator::Base { | ||||
|  | @ -1414,6 +1418,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { | |||
|   void serializable() const; | ||||
| 
 | ||||
|   double weight(double error) const; | ||||
|   double residual(double error) const; | ||||
| }; | ||||
| 
 | ||||
| virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { | ||||
|  | @ -1424,6 +1429,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { | |||
|   void serializable() const; | ||||
| 
 | ||||
|   double weight(double error) const; | ||||
|   double residual(double error) const; | ||||
| }; | ||||
| 
 | ||||
| //TODO DCS and L2WithDeadZone mEstimators
 | ||||
|  |  | |||
|  | @ -858,6 +858,12 @@ double GemanMcClure::weight(double error) const { | |||
|   return c4/(c2error*c2error); | ||||
| } | ||||
| 
 | ||||
| double GemanMcClure::residual(double error) const { | ||||
|   const double c2 = c_*c_; | ||||
|   const double error2 = error*error; | ||||
|   return 0.5 * (c2 * error2) / (c2 + error2); | ||||
| } | ||||
| 
 | ||||
| void GemanMcClure::print(const std::string &s="") const { | ||||
|   std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl; | ||||
| } | ||||
|  |  | |||
|  | @ -18,6 +18,7 @@ | |||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include<iostream> | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/Matrix.h> | ||||
| #include <gtsam/dllexport.h> | ||||
|  | @ -727,6 +728,7 @@ namespace gtsam { | |||
|         Null(const ReweightScheme reweight = Block) : Base(reweight) {} | ||||
|         virtual ~Null() {} | ||||
|         virtual double weight(double /*error*/) const { return 1.0; } | ||||
|         virtual double residual(double error) const { return error; } | ||||
|         virtual void print(const std::string &s) const; | ||||
|         virtual bool equals(const Base& /*expected*/, double /*tol*/) const { return true; } | ||||
|         static shared_ptr Create() ; | ||||
|  | @ -752,6 +754,12 @@ namespace gtsam { | |||
|         double weight(double error) const { | ||||
|           return 1.0 / (1.0 + std::abs(error) / c_); | ||||
|         } | ||||
|         double residual(double error) const { | ||||
|           double absError = std::abs(error); | ||||
|           double normalizedError = absError / c_; | ||||
|           double val = normalizedError - std::log(1 + normalizedError); | ||||
|           return c_ * c_ * val; | ||||
|         } | ||||
|         void print(const std::string &s) const; | ||||
|         bool equals(const Base& expected, double tol=1e-8) const; | ||||
|         static shared_ptr Create(double c, const ReweightScheme reweight = Block) ; | ||||
|  | @ -779,6 +787,14 @@ namespace gtsam { | |||
|           double absError = std::abs(error); | ||||
|           return (absError < k_) ? (1.0) : (k_ / absError); | ||||
|         } | ||||
|         double residual(double error) const { | ||||
|           double absError = std::abs(error); | ||||
|           if (absError <= k_) {  // |x| <= k
 | ||||
|             return error*error / 2; | ||||
|           } else { // |x| > k
 | ||||
|             return k_ * (absError - (k_/2)); | ||||
|           } | ||||
|         } | ||||
|         void print(const std::string &s) const; | ||||
|         bool equals(const Base& expected, double tol=1e-8) const; | ||||
|         static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; | ||||
|  | @ -809,6 +825,11 @@ namespace gtsam { | |||
|         double weight(double error) const { | ||||
|           return ksquared_ / (ksquared_ + error*error); | ||||
|         } | ||||
|         double residual(double error) const { | ||||
|           double normalizedError = error / k_; | ||||
|           double val = std::log(1 + (normalizedError*normalizedError)); | ||||
|           return ksquared_ * val * 0.5; | ||||
|         } | ||||
|         void print(const std::string &s) const; | ||||
|         bool equals(const Base& expected, double tol=1e-8) const; | ||||
|         static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; | ||||
|  | @ -839,6 +860,16 @@ namespace gtsam { | |||
|           } | ||||
|           return 0.0; | ||||
|         } | ||||
|         double residual(double error) const { | ||||
|           double absError = std::abs(error); | ||||
|           if (absError <= c_) { | ||||
|             double t = csquared_ - (error*error); | ||||
|             double val = t * t * t; | ||||
|             return ((csquared_*csquared_*csquared_) - val) / (6.0 * csquared_ * csquared_); | ||||
|           } else { | ||||
|             return csquared_ / 6.0; | ||||
|           } | ||||
|         } | ||||
|         void print(const std::string &s) const; | ||||
|         bool equals(const Base& expected, double tol=1e-8) const; | ||||
|         static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; | ||||
|  | @ -866,6 +897,10 @@ namespace gtsam { | |||
|           double xc2 = (error*error)/csquared_; | ||||
|           return std::exp(-xc2); | ||||
|         } | ||||
|         double residual(double error) const { | ||||
|           double xc2 = (error*error)/csquared_; | ||||
|           return csquared_ * 0.5 * (1 - std::exp(-xc2) ); | ||||
|         } | ||||
|         void print(const std::string &s) const; | ||||
|         bool equals(const Base& expected, double tol=1e-8) const; | ||||
|         static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; | ||||
|  | @ -897,6 +932,10 @@ namespace gtsam { | |||
|           double xc2 = (error*error)/csquared_; | ||||
|           return std::exp(-xc2); | ||||
|         } | ||||
|         double residual(double error) const { | ||||
|           double xc2 = (error*error)/csquared_; | ||||
|           return csquared_ * 0.5 * (1 - std::exp(-xc2) ); | ||||
|         } | ||||
|         void print(const std::string &s) const; | ||||
|         bool equals(const Base& expected, double tol=1e-8) const; | ||||
|         static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; | ||||
|  | @ -925,6 +964,7 @@ namespace gtsam { | |||
|         GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); | ||||
|         virtual ~GemanMcClure() {} | ||||
|         virtual double weight(double error) const; | ||||
|         virtual double residual(double error) const; | ||||
|         virtual void print(const std::string &s) const; | ||||
|         virtual bool equals(const Base& expected, double tol=1e-8) const; | ||||
|         static shared_ptr Create(double k, const ReweightScheme reweight = Block) ; | ||||
|  |  | |||
|  | @ -457,6 +457,22 @@ TEST(NoiseModel, WhitenInPlace) | |||
|  * penalties using iteratively re-weighted least squares. | ||||
|  */ | ||||
| 
 | ||||
| TEST(NoiseModel, robustFunctionFair) | ||||
| { | ||||
|   const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; | ||||
|   const mEstimator::Fair::shared_ptr fair = mEstimator::Fair::Create(k); | ||||
|   DOUBLES_EQUAL(0.8333333333333333, fair->weight(error1), 1e-8); | ||||
|   DOUBLES_EQUAL(0.3333333333333333, fair->weight(error2), 1e-8); | ||||
|   // Test negative value to ensure we take absolute value of error.
 | ||||
|   DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8); | ||||
| 
 | ||||
|   DOUBLES_EQUAL(0.441961080151135, fair->residual(error1), 1e-8); | ||||
|   DOUBLES_EQUAL(22.534692783297260, fair->residual(error2), 1e-8); | ||||
|   DOUBLES_EQUAL(22.534692783297260, fair->residual(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(0.441961080151135, fair->residual(error4), 1e-8); | ||||
| } | ||||
| 
 | ||||
| TEST(NoiseModel, robustFunctionHuber) | ||||
| { | ||||
|   const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; | ||||
|  | @ -466,16 +482,74 @@ TEST(NoiseModel, robustFunctionHuber) | |||
|   // Test negative value to ensure we take absolute value of error.
 | ||||
|   DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8); | ||||
| 
 | ||||
|   DOUBLES_EQUAL(0.5000, huber->residual(error1), 1e-8); | ||||
|   DOUBLES_EQUAL(37.5000, huber->residual(error2), 1e-8); | ||||
|   DOUBLES_EQUAL(37.5000, huber->residual(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(0.5000, huber->residual(error4), 1e-8); | ||||
| } | ||||
| 
 | ||||
| TEST(NoiseModel, robustFunctionCauchy) | ||||
| { | ||||
|   const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; | ||||
|   const mEstimator::Cauchy::shared_ptr cauchy = mEstimator::Cauchy::Create(k); | ||||
|   DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error1), 1e-8); | ||||
|   DOUBLES_EQUAL(0.2000, cauchy->weight(error2), 1e-8); | ||||
|   // Test negative value to ensure we take absolute value of error.
 | ||||
|   DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8); | ||||
| 
 | ||||
|   DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error1), 1e-8); | ||||
|   DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error2), 1e-8); | ||||
|   DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error4), 1e-8); | ||||
| } | ||||
| 
 | ||||
| TEST(NoiseModel, robustFunctionGemanMcClure) | ||||
| { | ||||
|   const double k = 1.0, error1 = 1.0, error2 = 10.0; | ||||
|   const double k = 1.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; | ||||
|   const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k); | ||||
|   const double weight1 = gmc->weight(error1), | ||||
|                weight2 = gmc->weight(error2); | ||||
|   DOUBLES_EQUAL(0.25      , weight1, 1e-8); | ||||
|   DOUBLES_EQUAL(9.80296e-5, weight2, 1e-8); | ||||
|   DOUBLES_EQUAL(0.25      , gmc->weight(error1), 1e-8); | ||||
|   DOUBLES_EQUAL(9.80296e-5, gmc->weight(error2), 1e-8); | ||||
|   DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(0.25      , gmc->weight(error4), 1e-8); | ||||
| 
 | ||||
|   DOUBLES_EQUAL(0.2500, gmc->residual(error1), 1e-8); | ||||
|   DOUBLES_EQUAL(0.495049504950495, gmc->residual(error2), 1e-8); | ||||
|   DOUBLES_EQUAL(0.495049504950495, gmc->residual(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(0.2500, gmc->residual(error4), 1e-8); | ||||
| } | ||||
| 
 | ||||
| TEST(NoiseModel, robustFunctionWelsch) | ||||
| { | ||||
|   const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; | ||||
|   const mEstimator::Welsch::shared_ptr welsch = mEstimator::Welsch::Create(k); | ||||
|   DOUBLES_EQUAL(0.960789439152323, welsch->weight(error1), 1e-8); | ||||
|   DOUBLES_EQUAL(0.018315638888734, welsch->weight(error2), 1e-8); | ||||
|   // Test negative value to ensure we take absolute value of error.
 | ||||
|   DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8); | ||||
| 
 | ||||
|   DOUBLES_EQUAL(0.490132010595960, welsch->residual(error1), 1e-8); | ||||
|   DOUBLES_EQUAL(12.271054513890823, welsch->residual(error2), 1e-8); | ||||
|   DOUBLES_EQUAL(12.271054513890823, welsch->residual(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(0.490132010595960, welsch->residual(error4), 1e-8); | ||||
| } | ||||
| 
 | ||||
| TEST(NoiseModel, robustFunctionTukey) | ||||
| { | ||||
|   const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; | ||||
|   const mEstimator::Tukey::shared_ptr tukey = mEstimator::Tukey::Create(k); | ||||
|   DOUBLES_EQUAL(0.9216, tukey->weight(error1), 1e-8); | ||||
|   DOUBLES_EQUAL(0.0, tukey->weight(error2), 1e-8); | ||||
|   // Test negative value to ensure we take absolute value of error.
 | ||||
|   DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8); | ||||
| 
 | ||||
|   DOUBLES_EQUAL(0.480266666666667, tukey->residual(error1), 1e-8); | ||||
|   DOUBLES_EQUAL(4.166666666666667, tukey->residual(error2), 1e-8); | ||||
|   DOUBLES_EQUAL(4.166666666666667, tukey->residual(error3), 1e-8); | ||||
|   DOUBLES_EQUAL(0.480266666666667, tukey->residual(error4), 1e-8); | ||||
| } | ||||
| 
 | ||||
| TEST(NoiseModel, robustFunctionDCS) | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue