Add custom unit test for Python robust function
							parent
							
								
									9c40a2cd8b
								
							
						
					
					
						commit
						7d04f4bce6
					
				|  | @ -0,0 +1,43 @@ | |||
| """ | ||||
| GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, | ||||
| Atlanta, Georgia 30332-0415 | ||||
| All Rights Reserved | ||||
| 
 | ||||
| See LICENSE for the license information | ||||
| 
 | ||||
| Simple unit test for custom robust noise model. | ||||
| Author: Fan Jiang | ||||
| """ | ||||
| import unittest | ||||
| 
 | ||||
| import gtsam | ||||
| import numpy as np | ||||
| from gtsam.utils.test_case import GtsamTestCase | ||||
| 
 | ||||
| 
 | ||||
| class TestRobust(GtsamTestCase): | ||||
| 
 | ||||
|     def test_RobustLossAndWeight(self): | ||||
|         k = 10.0 | ||||
| 
 | ||||
|         def custom_weight(e): | ||||
|             abs_e = abs(e) | ||||
|             return 1.0 if abs_e <= k else k / abs_e | ||||
| 
 | ||||
|         def custom_loss(e): | ||||
|             abs_e = abs(e) | ||||
|             return abs_e * abs_e / 2.0 if abs_e <= k else k * abs_e - k * k / 2.0 | ||||
| 
 | ||||
|         custom_robust = gtsam.noiseModel.Robust.Create( | ||||
|             gtsam.noiseModel.mEstimator.Custom(custom_weight, custom_loss, | ||||
|                                                gtsam.noiseModel.mEstimator.Base.ReweightScheme.Scalar, | ||||
|                                                "huber"), | ||||
|             gtsam.noiseModel.Isotropic.Sigma(1, 2.0)) | ||||
|         f = gtsam.PriorFactorDouble(0, 1.0, custom_robust) | ||||
|         v = gtsam.Values() | ||||
|         v.insert(0, 0.0) | ||||
| 
 | ||||
|         self.assertAlmostEquals(f.error(v), 0.125) | ||||
| 
 | ||||
| if __name__ == "__main__": | ||||
|     unittest.main() | ||||
		Loading…
	
		Reference in New Issue