Implemented Gaussian back-sub (also includes some cleanups from a few commits ago)
							parent
							
								
									2ce65f4cf8
								
							
						
					
					
						commit
						3def2db1a4
					
				|  | @ -29,6 +29,13 @@ namespace gtsam { | |||
|   /* ************************************************************************* */ | ||||
|   namespace internal | ||||
|   { | ||||
|     /* ************************************************************************* */ | ||||
|     struct OptimizeData { | ||||
|       boost::optional<OptimizeData&> parentData; | ||||
|       VectorValuesUnordered ancestorResults; | ||||
|       VectorValuesUnordered results; | ||||
|     }; | ||||
| 
 | ||||
|     /* ************************************************************************* */ | ||||
|     /** Pre-order visitor for back-substitution in a Bayes tree.  The visitor function operator()()
 | ||||
|      *  optimizes the clique given the solution for the parents, and returns the solution for the | ||||
|  | @ -36,34 +43,75 @@ namespace gtsam { | |||
|      *  solution that will finally be returned to the user.  The reason we pass the individual | ||||
|      *  clique solutions between nodes is to avoid log(n) lookups over all variables, they instead | ||||
|      *  then are only over a node's parent variables. */ | ||||
|     struct OptimizeClique | ||||
|     { | ||||
|       VectorValuesUnordered collectedResult; | ||||
|     //struct OptimizeClique
 | ||||
|     //{
 | ||||
|     //  VectorValuesUnordered collectedResult;
 | ||||
| 
 | ||||
|       VectorValuesUnordered operator()( | ||||
|         const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, | ||||
|         const VectorValuesUnordered& parentSolution) | ||||
|       { | ||||
|         // parents are assumed to already be solved and available in result
 | ||||
|         VectorValuesUnordered cliqueSolution = clique->conditional()->solve(parentSolution); | ||||
|         collectedResult.insert(cliqueSolution); | ||||
|         return cliqueSolution; | ||||
|       } | ||||
|     }; | ||||
|     //  VectorValuesUnordered operator()(
 | ||||
|     //    const GaussianBayesTreeCliqueUnordered::shared_ptr& clique,
 | ||||
|     //    const VectorValuesUnordered& parentSolution)
 | ||||
|     //  {
 | ||||
|     //    // parents are assumed to already be solved and available in result
 | ||||
|     //    VectorValuesUnordered cliqueSolution = clique->conditional()->solve(parentSolution);
 | ||||
|     //    collectedResult.insert(cliqueSolution);
 | ||||
|     //    return cliqueSolution;
 | ||||
|     //  }
 | ||||
|     //};
 | ||||
| 
 | ||||
|     /* ************************************************************************* */ | ||||
|     OptimizeData OptimizePreVisitor(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, OptimizeData& parentData) | ||||
|     { | ||||
|       // Create data - holds a pointer to our parent, a copy of parent solution, and our results
 | ||||
|       OptimizeData myData; | ||||
|       myData.parentData = parentData; | ||||
|       // Take any ancestor results we'll need
 | ||||
|       BOOST_FOREACH(Key parent, clique->conditional_->parents()) | ||||
|         myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]); | ||||
|       // Solve and store in our results
 | ||||
|       myData.results.insert(clique->conditional()->solve(myData.ancestorResults)); | ||||
|       myData.ancestorResults.insert(myData.results); | ||||
|       return myData; | ||||
|     } | ||||
| 
 | ||||
|     /* ************************************************************************* */ | ||||
|     void OptimizePostVisitor(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, OptimizeData& myData) | ||||
|     { | ||||
|       // Conglomerate our results to the parent
 | ||||
|       myData.parentData->results.insert(myData.results); | ||||
|     } | ||||
| 
 | ||||
|     /* ************************************************************************* */ | ||||
|     double logDeterminant(const GaussianBayesTreeCliqueUnordered::shared_ptr& clique, double& parentSum) | ||||
|     { | ||||
|       parentSum += clique->conditional()->get_R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum(); | ||||
|       assert(false); | ||||
|       return 0; | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   VectorValuesUnordered GaussianBayesTreeUnordered::optimize() const { | ||||
|     internal::OptimizeClique visitor; | ||||
|     VectorValuesUnordered empty; | ||||
|     treeTraversal::DepthFirstForest(*this, empty, visitor); | ||||
|     return visitor.collectedResult; | ||||
|   GaussianBayesTreeUnordered::GaussianBayesTreeUnordered(const GaussianBayesTreeUnordered& other) : | ||||
|     Base(other) {} | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   GaussianBayesTreeUnordered& GaussianBayesTreeUnordered::operator=(const GaussianBayesTreeUnordered& other) | ||||
|   { | ||||
|     (void) Base::operator=(other); | ||||
|     return *this; | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   bool GaussianBayesTreeUnordered::equals(const This& other, double tol) const | ||||
|   { | ||||
|     return Base::equals(other, tol); | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   VectorValuesUnordered GaussianBayesTreeUnordered::optimize() const | ||||
|   { | ||||
|     internal::OptimizeData rootData; // Will hold final solution
 | ||||
|     treeTraversal::DepthFirstForest(*this, rootData, internal::OptimizePreVisitor, internal::OptimizePostVisitor); | ||||
|     return rootData.results; | ||||
|   } | ||||
| 
 | ||||
|   ///* ************************************************************************* */
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue