Fixed bugs in marginal generation. It is possible that the marginalization will be "perfect", resulting in no marginal factor. However, a factor was being created anyway.
							parent
							
								
									584f5c6c8c
								
							
						
					
					
						commit
						3eade960b8
					
				|  | @ -454,12 +454,14 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) { | |||
|     NonlinearFactorGraph marginalFactors; | ||||
|     BOOST_FOREACH(Index index, indicesToEliminate) { | ||||
|       GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); | ||||
|       LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_)); | ||||
|       marginalFactors.push_back(marginalFactor); | ||||
|       // Add the keys associated with the marginal factor to the separator values
 | ||||
|       BOOST_FOREACH(Key key, *marginalFactor) { | ||||
|         if(!separatorValues_.exists(key)) { | ||||
|           separatorValues_.insert(key, theta_.at(key)); | ||||
|       if(gaussianFactor->size() > 0) { | ||||
|         LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_)); | ||||
|         marginalFactors.push_back(marginalFactor); | ||||
|         // Add the keys associated with the marginal factor to the separator values
 | ||||
|         BOOST_FOREACH(Key key, *marginalFactor) { | ||||
|           if(!separatorValues_.exists(key)) { | ||||
|             separatorValues_.insert(key, theta_.at(key)); | ||||
|           } | ||||
|         } | ||||
|       } | ||||
|     } | ||||
|  | @ -554,8 +556,10 @@ NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGra | |||
|   NonlinearFactorGraph marginalFactors; | ||||
|   BOOST_FOREACH(Index index, indicesToEliminate) { | ||||
|     GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(function); | ||||
|     LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values)); | ||||
|     marginalFactors.push_back(marginalFactor); | ||||
|     if(gaussianFactor->size() > 0) { | ||||
|       LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values)); | ||||
|       marginalFactors.push_back(marginalFactor); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   return marginalFactors; | ||||
|  |  | |||
|  | @ -75,27 +75,19 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearF | |||
|   } | ||||
|   gttoc(augment_system); | ||||
| 
 | ||||
|   // Reorder the system to ensure efficient optimization (and marginalization) performance
 | ||||
|   gttic(reorder); | ||||
|   reorder(); | ||||
|   gttoc(reorder); | ||||
| 
 | ||||
|   // Optimize the factors using a modified version of L-M
 | ||||
|   gttic(optimize); | ||||
|   factors_.print("factors_"); | ||||
|   theta_.print("theta_"); | ||||
|   if(factors_.size() > 0) { | ||||
|     // Reorder the system to ensure efficient optimization (and marginalization) performance
 | ||||
|     gttic(reorder); | ||||
|     reorder(); | ||||
|     gttoc(reorder); | ||||
| 
 | ||||
|     // Optimize the factors using a modified version of L-M
 | ||||
|     gttic(optimize); | ||||
|     result = optimize(); | ||||
|     gttoc(optimize); | ||||
|   } | ||||
|   gttoc(optimize); | ||||
| 
 | ||||
| 
 | ||||
|   // Moved presync code into the update function. Generally, only one call to smoother.update(*) is performed
 | ||||
|   // between synchronizations, so no extra work is being done. This also allows the presync code to be performed
 | ||||
|   // while the filter is still running (instead of during the synchronization when the filter is paused)
 | ||||
|   gttic(presync); | ||||
|   if(separatorValues_.size() > 0) { | ||||
|     updateSmootherSummarization(); | ||||
|   } | ||||
|   gttoc(presync); | ||||
| 
 | ||||
|   gttoc(update); | ||||
| 
 | ||||
|  | @ -414,8 +406,10 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() { | |||
|   // Convert the marginal factors into Linear Container Factors and store
 | ||||
|   BOOST_FOREACH(Index index, indicesToEliminate) { | ||||
|     GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); | ||||
|     LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_)); | ||||
|     smootherSummarization_.push_back(marginalFactor); | ||||
|     if(gaussianFactor->size() > 0) { | ||||
|       LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_)); | ||||
|       smootherSummarization_.push_back(marginalFactor); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
| } | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue