Fix conditional==null bug
							parent
							
								
									ed9a216365
								
							
						
					
					
						commit
						55ca557b1e
					
				|  | @ -40,7 +40,6 @@ | |||
| #include <gtsam/linear/HessianFactor.h> | ||||
| #include <gtsam/linear/JacobianFactor.h> | ||||
| 
 | ||||
| #include <algorithm> | ||||
| #include <cstddef> | ||||
| #include <iostream> | ||||
| #include <memory> | ||||
|  | @ -136,7 +135,9 @@ HybridGaussianFactorGraph::collectProductFactor() const { | |||
| 
 | ||||
|   for (auto &f : factors_) { | ||||
|     // TODO(dellaert): can we make this cleaner and less error-prone?
 | ||||
|     if (auto gf = dynamic_pointer_cast<GaussianFactor>(f)) { | ||||
|     if (auto orphan = dynamic_pointer_cast<OrphanWrapper>(f)) { | ||||
|       continue; // Ignore OrphanWrapper
 | ||||
|     } else if (auto gf = dynamic_pointer_cast<GaussianFactor>(f)) { | ||||
|       result += gf; | ||||
|     } else if (auto gc = dynamic_pointer_cast<GaussianConditional>(f)) { | ||||
|       result += gc; | ||||
|  | @ -269,15 +270,20 @@ static std::shared_ptr<Factor> createDiscreteFactor( | |||
|     const DiscreteKeys &discreteSeparator) { | ||||
|   auto negLogProbability = [&](const Result &pair) -> double { | ||||
|     const auto &[conditional, factor] = pair; | ||||
|     static const VectorValues kEmpty; | ||||
|     // If the factor is not null, it has no keys, just contains the residual.
 | ||||
|     if (!factor) return 1.0;  // TODO(dellaert): not loving this.
 | ||||
|     if (conditional && factor) { | ||||
|       static const VectorValues kEmpty; | ||||
|       // If the factor is not null, it has no keys, just contains the residual.
 | ||||
| 
 | ||||
|     // Negative logspace version of:
 | ||||
|     // exp(-factor->error(kEmpty)) / conditional->normalizationConstant();
 | ||||
|     // negLogConstant gives `-log(k)`
 | ||||
|     // which is `-log(k) = log(1/k) = log(\sqrt{|2πΣ|})`.
 | ||||
|     return factor->error(kEmpty) - conditional->negLogConstant(); | ||||
|       // Negative-log-space version of:
 | ||||
|       // exp(-factor->error(kEmpty)) / conditional->normalizationConstant();
 | ||||
|       // negLogConstant gives `-log(k)`
 | ||||
|       // which is `-log(k) = log(1/k) = log(\sqrt{|2πΣ|})`.
 | ||||
|       return factor->error(kEmpty) - conditional->negLogConstant(); | ||||
|     } else if (!conditional && !factor) { | ||||
|       return 1.0;  // TODO(dellaert): not loving this, what should this be??
 | ||||
|     } else { | ||||
|       throw std::runtime_error("createDiscreteFactor has mixed NULLs"); | ||||
|     } | ||||
|   }; | ||||
| 
 | ||||
|   AlgebraicDecisionTree<Key> negLogProbabilities( | ||||
|  | @ -296,15 +302,20 @@ static std::shared_ptr<Factor> createHybridGaussianFactor( | |||
|   // Correct for the normalization constant used up by the conditional
 | ||||
|   auto correct = [&](const Result &pair) -> GaussianFactorValuePair { | ||||
|     const auto &[conditional, factor] = pair; | ||||
|     if (factor) { | ||||
|     if (conditional && factor) { | ||||
|       auto hf = std::dynamic_pointer_cast<HessianFactor>(factor); | ||||
|       if (!hf) throw std::runtime_error("Expected HessianFactor!"); | ||||
|       // Add 2.0 term since the constant term will be premultiplied by 0.5
 | ||||
|       // as per the Hessian definition,
 | ||||
|       // and negative since we want log(k)
 | ||||
|       hf->constantTerm() += -2.0 * conditional->negLogConstant(); | ||||
|       const double negLogK = conditional->negLogConstant(); | ||||
|       hf->constantTerm() += -2.0 * negLogK; | ||||
|       return {factor, negLogK}; | ||||
|     } else if (!conditional && !factor){ | ||||
|       return {nullptr, 0.0};  // TODO(frank): or should this be infinity?
 | ||||
|     } else { | ||||
|         throw std::runtime_error("createHybridGaussianFactors has mixed NULLs"); | ||||
|     } | ||||
|     return {factor, conditional->negLogConstant()}; | ||||
|   }; | ||||
|   DecisionTree<Key, GaussianFactorValuePair> newFactors(eliminationResults, | ||||
|                                                         correct); | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue