done with new default noise thresholds!
							parent
							
								
									28b0f0ac8e
								
							
						
					
					
						commit
						a59a12245c
					
				|  | @ -65,10 +65,6 @@ class GncOptimizer { | |||
| 
 | ||||
|     // make sure all noiseModels are Gaussian or convert to Gaussian
 | ||||
|     nfg_.resize(graph.size()); | ||||
|     barcSq_ = Vector::Ones(graph.size()); | ||||
| 
 | ||||
|     double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_
 | ||||
| 
 | ||||
|     for (size_t i = 0; i < graph.size(); i++) { | ||||
|       if (graph[i]) { | ||||
|         NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast< | ||||
|  | @ -77,9 +73,12 @@ class GncOptimizer { | |||
|             noiseModel::Robust>(factor->noiseModel()); | ||||
|         // if the factor has a robust loss, we remove the robust loss
 | ||||
|         nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor; | ||||
|         barcSq_[i] = 0.5 * Chi2inv(alpha, nfg_[i]->dim()); // 0.5 derives from the error definition in gtsam
 | ||||
|       } | ||||
|     } | ||||
| 
 | ||||
|     // set default barcSq_ (inlier threshold)
 | ||||
|     double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_
 | ||||
|     setInlierCostThresholdsAtProbability(alpha); | ||||
|   } | ||||
| 
 | ||||
|   /** Set the maximum weighted residual error for an inlier (same for all factors). For a factor in the form f(x) = 0.5 * || r(x) ||^2_Omega,
 | ||||
|  | @ -102,6 +101,18 @@ class GncOptimizer { | |||
|     barcSq_ = inthVec; | ||||
|   } | ||||
| 
 | ||||
|   /** Set the maximum weighted residual error threshold by specifying the probability
 | ||||
|    * alpha that the inlier residuals are smaller than that threshold | ||||
|    * */ | ||||
|   void setInlierCostThresholdsAtProbability(const double alpha) { | ||||
|     barcSq_  = Vector::Ones(nfg_.size()); | ||||
|     for (size_t k = 0; k < nfg_.size(); k++) { | ||||
|       if (nfg_[k]) { | ||||
|         barcSq_[k] = 0.5 * Chi2inv(alpha, nfg_[k]->dim()); // 0.5 derives from the error definition in gtsam
 | ||||
|       } | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /// Access a copy of the internal factor graph.
 | ||||
|   const NonlinearFactorGraph& getFactors() const { return nfg_; } | ||||
| 
 | ||||
|  |  | |||
|  | @ -33,6 +33,9 @@ | |||
| #include <gtsam/slam/dataset.h> | ||||
| #include <tests/smallExample.h> | ||||
| 
 | ||||
| #include <gtsam/sam/BearingFactor.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
|  | @ -603,6 +606,59 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { | |||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(GncOptimizer, chi2inv) { | ||||
|   DOUBLES_EQUAL(8.807468393511950, Chi2inv(0.997, 1), tol); // from MATLAB: chi2inv(0.997, 1) = 8.807468393511950
 | ||||
|   DOUBLES_EQUAL(13.931422665512077, Chi2inv(0.997, 3), tol); // from MATLAB: chi2inv(0.997, 3) = 13.931422665512077
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(GncOptimizer, barcsq) { | ||||
|   auto fg = example::sharedNonRobustFactorGraphWithOutliers(); | ||||
| 
 | ||||
|   Point2 p0(1, 0); | ||||
|   Values initial; | ||||
|   initial.insert(X(1), p0); | ||||
| 
 | ||||
|   std::vector<size_t> knownInliers; | ||||
|   knownInliers.push_back(0); | ||||
|   knownInliers.push_back(1); | ||||
|   knownInliers.push_back(2); | ||||
| 
 | ||||
|   GncParams<GaussNewtonParams> gncParams; | ||||
|   gncParams.setKnownInliers(knownInliers); | ||||
|   gncParams.setLossType(GncLossType::GM); | ||||
|   //gncParams.setVerbosityGNC(GncParams<GaussNewtonParams>::Verbosity::SUMMARY);
 | ||||
|   auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial, | ||||
|       gncParams); | ||||
|   // expected: chi2inv(0.99, 2)/2
 | ||||
|   CHECK(assert_equal(4.605170185988091 * Vector::Ones(fg.size()), gnc.getInlierCostThresholds(), 1e-3)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(GncOptimizer, barcsq_heterogeneousFactors) { | ||||
|   NonlinearFactorGraph fg; | ||||
|   // specify noise model, otherwise it segfault if we leave default noise model
 | ||||
|    SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5)); | ||||
|    fg.add( PriorFactor<Pose2>(  0, Pose2(0.0, 0.0, 0.0) , model3D )); // size 3
 | ||||
|   SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5)); | ||||
|   fg.add( PriorFactor<Point2>(  1, Point2(0.0,0.0), model2D )); // size 2
 | ||||
|   SharedNoiseModel model1D(noiseModel::Isotropic::Sigma(1, 0.5)); | ||||
|   fg.add( BearingFactor<Pose2, Point2>(  0, 1, 1.0, model1D) ); // size 1
 | ||||
| 
 | ||||
|   Values initial; | ||||
|   initial.insert(0, Pose2(0.0, 0.0, 0.0)); | ||||
|   initial.insert(1, Point2(0.0,0.0)); | ||||
| 
 | ||||
|   auto gnc = GncOptimizer<GncParams<GaussNewtonParams>>(fg, initial); | ||||
|   CHECK(assert_equal(Vector3(5.672433365072185, 4.605170185988091, 3.317448300510607), | ||||
|                      gnc.getInlierCostThresholds(), 1e-3)); | ||||
| 
 | ||||
|   // extra test:
 | ||||
|   // fg.add( PriorFactor<Pose2>(  0, Pose2(0.0, 0.0, 0.0) )); // works if we add model3D as noise model
 | ||||
|   // std::cout <<  "fg[3]->dim() " << fg[3]->dim() << std::endl; // this segfaults?
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(GncOptimizer, setWeights) { | ||||
|   auto fg = example::sharedNonRobustFactorGraphWithOutliers(); | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue