Merge pull request #1390 from borglab/hybrid/simplify-2
						commit
						3460147df8
					
				|  | @ -299,19 +299,19 @@ void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) { | |||
| /* *******************************************************************************/ | ||||
| AlgebraicDecisionTree<Key> GaussianMixture::logProbability( | ||||
|     const VectorValues &continuousValues) const { | ||||
|   // functor to calculate to double logProbability value from
 | ||||
|   // functor to calculate (double) logProbability value from
 | ||||
|   // GaussianConditional.
 | ||||
|   auto errorFunc = | ||||
|   auto probFunc = | ||||
|       [continuousValues](const GaussianConditional::shared_ptr &conditional) { | ||||
|         if (conditional) { | ||||
|           return conditional->logProbability(continuousValues); | ||||
|         } else { | ||||
|           // Return arbitrarily large logProbability if conditional is null
 | ||||
|           // Return arbitrarily small logProbability if conditional is null
 | ||||
|           // Conditional is null if it is pruned out.
 | ||||
|           return 1e50; | ||||
|           return -1e20; | ||||
|         } | ||||
|       }; | ||||
|   return DecisionTree<Key, double>(conditionals_, errorFunc); | ||||
|   return DecisionTree<Key, double>(conditionals_, probFunc); | ||||
| } | ||||
| 
 | ||||
| /* *******************************************************************************/ | ||||
|  |  | |||
|  | @ -76,13 +76,16 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc( | |||
|   auto pruner = [prunedDecisionTree, decisionTreeKeySet, conditionalKeySet]( | ||||
|                     const Assignment<Key> &choices, | ||||
|                     double probability) -> double { | ||||
|     // This corresponds to 0 probability
 | ||||
|     double pruned_prob = 0.0; | ||||
| 
 | ||||
|     // typecast so we can use this to get probability value
 | ||||
|     DiscreteValues values(choices); | ||||
|     // Case where the Gaussian mixture has the same
 | ||||
|     // discrete keys as the decision tree.
 | ||||
|     if (conditionalKeySet == decisionTreeKeySet) { | ||||
|       if (prunedDecisionTree(values) == 0) { | ||||
|         return 0.0; | ||||
|         return pruned_prob; | ||||
|       } else { | ||||
|         return probability; | ||||
|       } | ||||
|  | @ -133,7 +136,7 @@ std::function<double(const Assignment<Key> &, double)> prunerFunc( | |||
|       } | ||||
|       // If we are here, it means that all the sub-branches are 0,
 | ||||
|       // so we prune.
 | ||||
|       return 0.0; | ||||
|       return pruned_prob; | ||||
|     } | ||||
|   }; | ||||
|   return pruner; | ||||
|  |  | |||
|  | @ -39,6 +39,7 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, | |||
|   if (newFactors.size() > 0) { | ||||
|     // Reorder and relinearize every reorderInterval updates
 | ||||
|     if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) { | ||||
|       // TODO(Varun) Relinearization doesn't take into account pruning
 | ||||
|       reorder_relinearize(); | ||||
|       reorderCounter_ = 0; | ||||
|     } | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue