Finishing touches
							parent
							
								
									e59b3afc29
								
							
						
					
					
						commit
						07b4c236eb
					
				|  | @ -29,6 +29,7 @@ | |||
| #include <gtsam/hybrid/HybridValues.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/linear/GaussianFactorGraph.h> | ||||
| #include <gtsam/linear/VectorValues.h> | ||||
| #include <gtsam/nonlinear/PriorFactor.h> | ||||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| 
 | ||||
|  | @ -433,20 +434,20 @@ HybridBayesNet CreateBayesNet( | |||
|   return hbn; | ||||
| } | ||||
| 
 | ||||
| /// Create importance sampling network p(x1| x0, m1) p(x0) P(m1),
 | ||||
| /// using Q(x0) = N(z0, sigma_Q) to sample from p(x0)
 | ||||
| /// Create importance sampling network q(x0,x1,m) = p(x1|x0,m1) q(x0) P(m1),
 | ||||
| /// using q(x0) = N(z0, sigma_Q) to sample x0.
 | ||||
| HybridBayesNet CreateProposalNet( | ||||
|     const GaussianMixture::shared_ptr& hybridMotionModel, double z0, | ||||
|     const GaussianMixture::shared_ptr& hybridMotionModel, const Vector1& z0, | ||||
|     double sigma_Q) { | ||||
|   HybridBayesNet hbn; | ||||
| 
 | ||||
|   // Add hybrid motion model
 | ||||
|   hbn.push_back(hybridMotionModel); | ||||
| 
 | ||||
|   // Add proposal Q(x0) for x0
 | ||||
|   // Add proposal q(x0) for x0
 | ||||
|   auto measurement_model = noiseModel::Isotropic::Sigma(1, sigma_Q); | ||||
|   hbn.emplace_shared<GaussianConditional>( | ||||
|       GaussianConditional::FromMeanAndStddev(X(0), Vector1(z0), sigma_Q)); | ||||
|       GaussianConditional::FromMeanAndStddev(X(0), z0, sigma_Q)); | ||||
| 
 | ||||
|   // Discrete uniform prior.
 | ||||
|   hbn.emplace_shared<DiscreteConditional>(m1, "0.5/0.5"); | ||||
|  | @ -466,7 +467,7 @@ void approximateDiscreteMarginal(const HybridBayesNet& hbn, | |||
|     HybridValues sample = proposalNet.sample(&rng); | ||||
|     sample.insert(given); | ||||
|     double weight = hbn.evaluate(sample) / proposalNet.evaluate(sample); | ||||
|     (sample.atDiscrete(m1.first) == 0) ? w0 += weight : w1 += weight; | ||||
|     (sample.atDiscrete(M(1)) == 0) ? w0 += weight : w1 += weight; | ||||
|   } | ||||
|   double sumWeights = w0 + w1; | ||||
|   double pm1 = w1 / sumWeights; | ||||
|  | @ -478,15 +479,14 @@ void approximateDiscreteMarginal(const HybridBayesNet& hbn, | |||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| /**
 | ||||
|  * Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1). | ||||
|  * Test a model p(z0|x0)p(z1|x1)p(x1|x0,m1)P(m1). | ||||
|  * | ||||
|  * P(f01|x1,x0,m1) has different means and same covariance. | ||||
|  * p(x1|x0,m1) has mode-dependent mean but same covariance. | ||||
|  * | ||||
|  * Converting to a factor graph gives us | ||||
|  * ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1) | ||||
|  * Converting to a factor graph gives us ϕ(x0;z0)ϕ(x1;z1)ϕ(x1,x0,m1)P(m1) | ||||
|  * | ||||
|  * If we only have a measurement on z0, then | ||||
|  * the probability of m1 should be 0.5/0.5. | ||||
|  * If we only have a measurement on x0, then | ||||
|  * the posterior probability of m1 should be 0.5/0.5. | ||||
|  * Getting a measurement on z1 gives use more information. | ||||
|  */ | ||||
| TEST(GaussianMixtureFactor, TwoStateModel) { | ||||
|  | @ -497,10 +497,10 @@ TEST(GaussianMixtureFactor, TwoStateModel) { | |||
|   auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma, sigma); | ||||
| 
 | ||||
|   // Start with no measurement on x1, only on x0
 | ||||
|   double z0 = 0.5; | ||||
|   const Vector1 z0(0.5); | ||||
| 
 | ||||
|   VectorValues given; | ||||
|   given.insert(Z(0), Vector1(z0)); | ||||
|   given.insert(Z(0), z0); | ||||
| 
 | ||||
|   // Create proposal network for importance sampling
 | ||||
|   auto proposalNet = CreateProposalNet(hybridMotionModel, z0, 3.0); | ||||
|  | @ -522,10 +522,10 @@ TEST(GaussianMixtureFactor, TwoStateModel) { | |||
|     // Now we add a measurement z1 on x1
 | ||||
|     HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); | ||||
| 
 | ||||
|     // If we see z1=4.5 (>> 2.5 which is the halfway point),
 | ||||
|     // discrete mode should say m1=1
 | ||||
|     const double z1 = 4.5; | ||||
|     given.insert(Z(1), Vector1(z1)); | ||||
|     // If we set z1=4.5 (>> 2.5 which is the halfway point),
 | ||||
|     // probability of discrete mode should be leaning to m1==1.
 | ||||
|     const Vector1 z1(4.5); | ||||
|     given.insert(Z(1), z1); | ||||
| 
 | ||||
|     HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); | ||||
|     HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); | ||||
|  | @ -534,7 +534,7 @@ TEST(GaussianMixtureFactor, TwoStateModel) { | |||
|     // Values taken from an importance sampling run with 50k samples:
 | ||||
|     // approximateDiscreteMarginal(hbn, proposalNet, given);
 | ||||
|     DiscreteConditional expected(m1, "0.446629/0.553371"); | ||||
|     EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.01)); | ||||
|     EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.002)); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
|  | @ -559,9 +559,9 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { | |||
|   auto hybridMotionModel = CreateHybridMotionModel(mu0, mu1, sigma0, sigma1); | ||||
| 
 | ||||
|   // Start with no measurement on x1, only on x0
 | ||||
|   double z0 = 0.5; | ||||
|   const Vector1 z0(0.5); | ||||
|   VectorValues given; | ||||
|   given.insert(Z(0), Vector1(z0)); | ||||
|   given.insert(Z(0), z0); | ||||
| 
 | ||||
|   // Create proposal network for importance sampling
 | ||||
|   // uncomment this and the approximateDiscreteMarginal calls to run
 | ||||
|  | @ -571,19 +571,13 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { | |||
|     HybridBayesNet hbn = CreateBayesNet(hybridMotionModel); | ||||
|     HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); | ||||
| 
 | ||||
|     { | ||||
|       VectorValues vv{ | ||||
|           {X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}, {Z(0), Vector1(z0)}}; | ||||
|       HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), | ||||
|           hv1(vv, DiscreteValues{{M(1), 1}}); | ||||
|       EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), | ||||
|                            gfg.error(hv1) / hbn.error(hv1), 1e-9); | ||||
|     } | ||||
|     { | ||||
|       VectorValues vv{ | ||||
|           {X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}, {Z(0), Vector1(z0)}}; | ||||
|       HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), | ||||
|           hv1(vv, DiscreteValues{{M(1), 1}}); | ||||
|     // Check that ratio of Bayes net and factor graph for different modes is
 | ||||
|     // equal for several values of {x0,x1}.
 | ||||
|     for (VectorValues vv : | ||||
|          {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, | ||||
|           VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { | ||||
|       vv.insert(given);  // add measurements for HBN
 | ||||
|       HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}}); | ||||
|       EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), | ||||
|                            gfg.error(hv1) / hbn.error(hv1), 1e-9); | ||||
|     } | ||||
|  | @ -595,66 +589,54 @@ TEST(GaussianMixtureFactor, TwoStateModel2) { | |||
| 
 | ||||
|     // Since no measurement on x1, we a 50/50 probability
 | ||||
|     auto p_m = bn->at(2)->asDiscrete(); | ||||
|     EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()(DiscreteValues{{m1.first, 0}}), | ||||
|                          1e-9); | ||||
|     EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()(DiscreteValues{{m1.first, 1}}), | ||||
|                          1e-9); | ||||
|     EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()(DiscreteValues{{M(1), 0}}), 1e-9); | ||||
|     EXPECT_DOUBLES_EQUAL(0.5, p_m->operator()(DiscreteValues{{M(1), 1}}), 1e-9); | ||||
|   } | ||||
| 
 | ||||
|   { | ||||
|     // Now we add a measurement z1 on x1
 | ||||
|     HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); | ||||
| 
 | ||||
|     double z1 = 4.0;  // favors m==1
 | ||||
|     given.insert(Z(1), Vector1(z1)); | ||||
|     const Vector1 z1(4.0);  // favors m==1
 | ||||
|     given.insert(Z(1), z1); | ||||
| 
 | ||||
|     HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); | ||||
| 
 | ||||
|     { | ||||
|       VectorValues vv{{X(0), Vector1(0.0)}, | ||||
|                       {X(1), Vector1(1.0)}, | ||||
|                       {Z(0), Vector1(z0)}, | ||||
|                       {Z(1), Vector1(z1)}}; | ||||
|       HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), | ||||
|           hv1(vv, DiscreteValues{{M(1), 1}}); | ||||
|       EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), | ||||
|                            gfg.error(hv1) / hbn.error(hv1), 1e-9); | ||||
|     } | ||||
|     { | ||||
|       VectorValues vv{{X(0), Vector1(0.5)}, | ||||
|                       {X(1), Vector1(3.0)}, | ||||
|                       {Z(0), Vector1(z0)}, | ||||
|                       {Z(1), Vector1(z1)}}; | ||||
|       HybridValues hv0(vv, DiscreteValues{{M(1), 0}}), | ||||
|           hv1(vv, DiscreteValues{{M(1), 1}}); | ||||
|     // Check that ratio of Bayes net and factor graph for different modes is
 | ||||
|     // equal for several values of {x0,x1}.
 | ||||
|     for (VectorValues vv : | ||||
|          {VectorValues{{X(0), Vector1(0.0)}, {X(1), Vector1(1.0)}}, | ||||
|           VectorValues{{X(0), Vector1(0.5)}, {X(1), Vector1(3.0)}}}) { | ||||
|       vv.insert(given);  // add measurements for HBN
 | ||||
|       HybridValues hv0(vv, {{M(1), 0}}), hv1(vv, {{M(1), 1}}); | ||||
|       EXPECT_DOUBLES_EQUAL(gfg.error(hv0) / hbn.error(hv0), | ||||
|                            gfg.error(hv1) / hbn.error(hv1), 1e-9); | ||||
|     } | ||||
| 
 | ||||
|     HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); | ||||
| 
 | ||||
|     // Since we have a measurement on z2, we get a definite result
 | ||||
|     // Since we have a measurement z1 on x1, we get a definite result
 | ||||
|     // Values taken from an importance sampling run with 50k samples:
 | ||||
|     // approximateDiscreteMarginal(hbn, proposalNet, given);
 | ||||
|     DiscreteConditional expected(m1, "0.481793/0.518207"); | ||||
|     EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.01)); | ||||
|     EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.001)); | ||||
|   } | ||||
| 
 | ||||
|   { | ||||
|     // Add a different measurement z1 on that favors m==0
 | ||||
|     // Add a different measurement z1 on x1 that favors m==0
 | ||||
|     HybridBayesNet hbn = CreateBayesNet(hybridMotionModel, true); | ||||
| 
 | ||||
|     double z1 = 1.1; | ||||
|     given.insert_or_assign(Z(1), Vector1(z1)); | ||||
|     const Vector1 z1(1.1); | ||||
|     given.insert_or_assign(Z(1), z1); | ||||
| 
 | ||||
|     HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); | ||||
|     HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); | ||||
| 
 | ||||
|     // Since we have a measurement on z2, we get a definite result
 | ||||
|     // Since we have a measurement z1 on x1, we get a definite result
 | ||||
|     // Values taken from an importance sampling run with 50k samples:
 | ||||
|     // approximateDiscreteMarginal(hbn, proposalNet, given);
 | ||||
|     DiscreteConditional expected(m1, "0.554485/0.445515"); | ||||
|     EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.01)); | ||||
|     EXPECT(assert_equal(expected, *(bn->at(2)->asDiscrete()), 0.001)); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue