commit
						19315cc3f3
					
				|  | @ -629,7 +629,7 @@ namespace gtsam { | |||
|     /**
 | ||||
|      * The mEstimator name space contains all robust error functions. | ||||
|      * It mirrors the exposition at | ||||
|      *  http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html
 | ||||
|      *  https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
 | ||||
|      * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. | ||||
|      * | ||||
|      * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: | ||||
|  | @ -681,7 +681,7 @@ namespace gtsam { | |||
|         /*
 | ||||
|          * This method is responsible for returning the weight function for a given amount of error. | ||||
|          * The weight function is related to the analytic derivative of the residual function. See | ||||
|          *  http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html
 | ||||
|          *  https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
 | ||||
|          * for details. This method is required when optimizing cost functions with robust penalties | ||||
|          * using iteratively re-weighted least squares. | ||||
|          */ | ||||
|  | @ -776,7 +776,8 @@ namespace gtsam { | |||
| 
 | ||||
|         Huber(double k = 1.345, const ReweightScheme reweight = Block); | ||||
|         double weight(double error) const { | ||||
|           return (error < k_) ? (1.0) : (k_ / fabs(error)); | ||||
|           double absError = std::abs(error); | ||||
|           return (absError < k_) ? (1.0) : (k_ / absError); | ||||
|         } | ||||
|         void print(const std::string &s) const; | ||||
|         bool equals(const Base& expected, double tol=1e-8) const; | ||||
|  |  | |||
|  | @ -452,19 +452,20 @@ TEST(NoiseModel, WhitenInPlace) | |||
| /*
 | ||||
|  * These tests are responsible for testing the weight functions for the m-estimators in GTSAM. | ||||
|  * The weight function is related to the analytic derivative of the residual function. See | ||||
|  *  http://research.microsoft.com/en-us/um/people/zhang/INRIA/Publis/Tutorial-Estim/node24.html
 | ||||
|  *  https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
 | ||||
|  * for details. This weight function is required when optimizing cost functions with robust | ||||
|  * penalties using iteratively re-weighted least squares. | ||||
|  */ | ||||
| 
 | ||||
| TEST(NoiseModel, robustFunctionHuber) | ||||
| { | ||||
|   const double k = 5.0, error1 = 1.0, error2 = 10.0; | ||||
|   const double k = 5.0, error1 = 1.0, error2 = 10.0, error3 = -10.0, error4 = -1.0; | ||||
|   const mEstimator::Huber::shared_ptr huber = mEstimator::Huber::Create(k); | ||||
|   const double weight1 = huber->weight(error1), | ||||
|                weight2 = huber->weight(error2); | ||||
|   DOUBLES_EQUAL(1.0, weight1, 1e-8); | ||||
|   DOUBLES_EQUAL(0.5, weight2, 1e-8); | ||||
|   DOUBLES_EQUAL(1.0, huber->weight(error1), 1e-8); | ||||
|   DOUBLES_EQUAL(0.5, huber->weight(error2), 1e-8); | ||||
|   // 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); | ||||
| } | ||||
| 
 | ||||
| TEST(NoiseModel, robustFunctionGemanMcClure) | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue