From 48fc15552afe256b01c9842056678e831c2b792a Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 21 May 2013 17:50:04 +0000 Subject: [PATCH 01/47] Moved key utilty functions back to MastSLAM --- gtsam.h | 5 ----- gtsam/nonlinear/Key.cpp | 33 --------------------------------- gtsam/nonlinear/Key.h | 9 --------- 3 files changed, 47 deletions(-) diff --git a/gtsam.h b/gtsam.h index 0c393a433..dbd3d9fad 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1450,11 +1450,6 @@ size_t symbol(char chr, size_t index); char symbolChr(size_t key); size_t symbolIndex(size_t key); -// Key utilities -gtsam::KeySet keyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB); -gtsam::KeySet keyDifference(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB); -bool hasKeyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB); - // Default keyformatter void printKeySet(const gtsam::KeySet& keys); void printKeySet(const gtsam::KeySet& keys, string s); diff --git a/gtsam/nonlinear/Key.cpp b/gtsam/nonlinear/Key.cpp index fe9f8c480..c4b21e681 100644 --- a/gtsam/nonlinear/Key.cpp +++ b/gtsam/nonlinear/Key.cpp @@ -60,39 +60,6 @@ void printKeySet(const gtsam::KeySet& keys, const std::string& s, const KeyForma std::cout << std::endl; } } - -/* ************************************************************************* */ -gtsam::KeySet keyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB) { - gtsam::KeySet intersection; - if (keysA.empty() || keysB.empty()) - return intersection; - BOOST_FOREACH(const gtsam::Key& key, keysA) - if (keysB.count(key)) - intersection.insert(key); - return intersection; -} - -/* ************************************************************************* */ -bool hasKeyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB) { - if (keysA.empty() || keysB.empty()) - return false; - BOOST_FOREACH(const gtsam::Key& key, keysA) - if (keysB.count(key)) - return true; - return false; -} - -/* ************************************************************************* */ -gtsam::KeySet keyDifference(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB) { - if (keysA.empty() || keysB.empty()) - return keysA; - - gtsam::KeySet difference; - BOOST_FOREACH(const gtsam::Key& key, keysA) - if (!keysB.count(key)) - difference.insert(key); - return difference; -} /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/nonlinear/Key.h b/gtsam/nonlinear/Key.h index bcc08d9cd..edeea396f 100644 --- a/gtsam/nonlinear/Key.h +++ b/gtsam/nonlinear/Key.h @@ -60,14 +60,5 @@ namespace gtsam { /// Utility function to print sets of keys with optional prefix GTSAM_EXPORT void printKeySet(const KeySet& keys, const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); - - /// Computes the intersection between two sets - GTSAM_EXPORT gtsam::KeySet keyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB); - - /// Checks if an intersection exists - faster checking size of above - GTSAM_EXPORT bool hasKeyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB); - - /// Computes a difference between sets, so result is those that are in A, but not B - GTSAM_EXPORT gtsam::KeySet keyDifference(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB); } From 5f371a4e55072ff73910467558b6cfe9be910482 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Tue, 21 May 2013 21:07:43 +0000 Subject: [PATCH 02/47] Modified Concurrent Filter to calculate marginals using a "shortcut" that allows constant-time updates during synchronization. Still need to test implementation. --- .../nonlinear/ConcurrentBatchFilter.cpp | 621 ++++++++++++------ .../nonlinear/ConcurrentBatchFilter.h | 20 +- 2 files changed, 450 insertions(+), 191 deletions(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 1e1e39e10..d8dcd6c14 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -85,11 +85,11 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto } gttoc(optimize); - gttic(marginalize); + gttic(move_separator); if(keysToMove && keysToMove->size() > 0){ - marginalize(*keysToMove); + moveSeparator(*keysToMove); } - gttoc(marginalize); + gttoc(move_separator); gttoc(update); @@ -109,100 +109,169 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFa gttic(synchronize); - // Remove the previous smoother summarization - removeFactors(smootherSummarizationSlots_); + // Update the smoother summarization on the old separator + previousSmootherSummarization_ = summarizedFactors; - // Create a factor graph containing the new smoother summarization, the factors to be sent to the smoother, - // and all of the filter factors. This is the set of factors on the filter side since the smoother started - // its previous update cycle. - NonlinearFactorGraph graph; - graph.push_back(factors_); - graph.push_back(smootherFactors_); - graph.push_back(summarizedFactors); - Values values; - values.insert(theta_); - values.insert(smootherValues_); - values.update(separatorValues); // ensure the smoother summarized factors are linearized around the values in the smoother + // Use the shortcut to calculate an updated marginal on the current separator + { + // Combine the new smoother summarization and the existing shortcut + NonlinearFactorGraph graph; + graph.push_back(previousSmootherSummarization_); + graph.push_back(smootherShortcut_); - if(factors_.size() > 0) { - // Perform an optional optimization on the to-be-sent-to-the-smoother factors - if(relin_) { - // Create ordering and delta - Ordering ordering = *graph.orderingCOLAMD(values); - VectorValues delta = values.zeroVectors(ordering); - // Optimize this graph using a modified version of L-M - optimize(graph, values, ordering, delta, separatorValues, parameters_); - // Update filter theta and delta - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { - theta_.update(key_value.key, values.at(key_value.key)); - delta_.at(ordering_.at(key_value.key)) = delta.at(ordering.at(key_value.key)); + // Extract the values needed for just this graph + Values values; + BOOST_FOREACH(Key key, graph.keys()) { + values.insert(key, theta_.at(key)); + } + + // Calculate the ordering: [OldSeparator NewSeparator] + // And determine the set of variables to be marginalized out + std::map constraints; + std::set marginalizeKeys; + int group = 0; + if(values.size() > 0) { + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, values) { + constraints[key_value.key] = group; + marginalizeKeys.insert(key_value.key); } - // Update the fixed linearization points (since they just changed) + ++group; + } + if(separatorValues_.size() > 0) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - separatorValues_.update(key_value.key, values.at(key_value.key)); + constraints[key_value.key] = group; + marginalizeKeys.erase(key_value.key); } } + Ordering ordering = *graph.orderingCOLAMDConstrained(values, constraints); - // Create separate ordering constraints that force either the filter keys or the smoother keys to the front - typedef std::map OrderingConstraints; - OrderingConstraints filterConstraints; - OrderingConstraints smootherConstraints; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { /// the filter keys - filterConstraints[key_value.key] = 0; - smootherConstraints[key_value.key] = 1; - } - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { /// the smoother keys - filterConstraints[key_value.key] = 1; - smootherConstraints[key_value.key] = 0; - } - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { /// the *new* separator keys - filterConstraints[key_value.key] = 2; - smootherConstraints[key_value.key] = 2; - } + // Calculate the marginal on the new separator (from the smoother side) + NonlinearFactorGraph marginals = marginalize(graph, values, ordering, marginalizeKeys, parameters_.getEliminationFunction()); - // Generate separate orderings that place the filter keys or the smoother keys first - // TODO: This is convenient, but it recalculates the variable index each time - Ordering filterOrdering = *graph.orderingCOLAMDConstrained(values, filterConstraints); - Ordering smootherOrdering = *graph.orderingCOLAMDConstrained(values, smootherConstraints); - - // Extract the set of filter keys and smoother keys - std::set filterKeys; - std::set separatorKeys; - std::set smootherKeys; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { - filterKeys.insert(key_value.key); - } - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - separatorKeys.insert(key_value.key); - filterKeys.erase(key_value.key); - } - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { - smootherKeys.insert(key_value.key); - } - - // Calculate the marginal on the new separator from the filter factors. This is performed by marginalizing out - // all of the filter variables that are not part of the new separator. This filter summarization will then be - // sent to the smoother. - filterSummarization_ = marginalize(graph, values, filterOrdering, filterKeys, parameters_.getEliminationFunction()); - // The filter summarization should also include any nonlinear factors that involve only the separator variables. - // Otherwise the smoother will be missing this information - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) { - if(factor) { - NonlinearFactor::const_iterator key = factor->begin(); - while((key != factor->end()) && (std::binary_search(separatorKeys.begin(), separatorKeys.end(), *key))) { - ++key; - } - if(key == factor->end()) { - filterSummarization_.push_back(factor); - } - } - } - - // Calculate the marginal on the new separator from the smoother factors. This is performed by marginalizing out - // all of the smoother variables that are not part of the new separator. This smoother summarization will be - // stored locally for use in the filter - smootherSummarizationSlots_ = insertFactors( marginalize(graph, values, smootherOrdering, smootherKeys, parameters_.getEliminationFunction()) ); + // Remove old summarization and insert new + removeFactors(currentSmootherSummarizationSlots_); + currentSmootherSummarizationSlots_ = insertFactors(marginals); } + + // Calculate the marginal on the new separator from the filter factors + // Note: This could also be done during each filter update so it would simply be available + { + // Calculate an ordering that places the new separator at the root + // And determine the set of variables to be marginalized out + std::map constraints; + std::set marginalizeKeys; + int group = 0; + if(theta_.size() > 0) { + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { + constraints[key_value.key] = group; + marginalizeKeys.insert(key_value.key); + } + ++group; + } + if(separatorValues_.size() > 0) { + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + constraints[key_value.key] = group; + marginalizeKeys.erase(key_value.key); + } + } + Ordering ordering = *factors_.orderingCOLAMDConstrained(theta_, constraints); + + // Calculate the marginal on the new separator (from the filter side) + filterSummarization_ = marginalize(factors_, theta_, ordering, marginalizeKeys, parameters_.getEliminationFunction()); + } + +// // Create a factor graph containing the new smoother summarization, the factors to be sent to the smoother, +// // and all of the filter factors. This is the set of factors on the filter side since the smoother started +// // its previous update cycle. +// NonlinearFactorGraph graph; +// graph.push_back(factors_); +// graph.push_back(smootherFactors_); +// graph.push_back(summarizedFactors); +// Values values; +// values.insert(theta_); +// values.insert(smootherValues_); +// values.update(separatorValues); // ensure the smoother summarized factors are linearized around the values in the smoother +// +// if(factors_.size() > 0) { +// // Perform an optional optimization on the to-be-sent-to-the-smoother factors +// if(true) { +// // Create ordering and delta +// Ordering ordering = *graph.orderingCOLAMD(values); +// VectorValues delta = values.zeroVectors(ordering); +// // Optimize this graph using a modified version of L-M +// optimize(graph, values, ordering, delta, separatorValues, parameters_); +// // Update filter theta and delta +// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { +// theta_.update(key_value.key, values.at(key_value.key)); +// delta_.at(ordering_.at(key_value.key)) = delta.at(ordering.at(key_value.key)); +// } +// // Update the fixed linearization points (since they just changed) +// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { +// separatorValues_.update(key_value.key, values.at(key_value.key)); +// } +// } +// +// // Create separate ordering constraints that force either the filter keys or the smoother keys to the front +// typedef std::map OrderingConstraints; +// OrderingConstraints filterConstraints; +// OrderingConstraints smootherConstraints; +// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { /// the filter keys +// filterConstraints[key_value.key] = 0; +// smootherConstraints[key_value.key] = 1; +// } +// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { /// the smoother keys +// filterConstraints[key_value.key] = 1; +// smootherConstraints[key_value.key] = 0; +// } +// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { /// the *new* separator keys +// filterConstraints[key_value.key] = 2; +// smootherConstraints[key_value.key] = 2; +// } +// +// // Generate separate orderings that place the filter keys or the smoother keys first +// // TODO: This is convenient, but it recalculates the variable index each time +// Ordering filterOrdering = *graph.orderingCOLAMDConstrained(values, filterConstraints); +// Ordering smootherOrdering = *graph.orderingCOLAMDConstrained(values, smootherConstraints); +// +// // Extract the set of filter keys and smoother keys +// std::set filterKeys; +// std::set separatorKeys; +// std::set smootherKeys; +// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { +// filterKeys.insert(key_value.key); +// } +// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { +// separatorKeys.insert(key_value.key); +// filterKeys.erase(key_value.key); +// } +// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { +// smootherKeys.insert(key_value.key); +// } +// +// // Calculate the marginal on the new separator from the filter factors. This is performed by marginalizing out +// // all of the filter variables that are not part of the new separator. This filter summarization will then be +// // sent to the smoother. +// filterSummarization_ = marginalize(graph, values, filterOrdering, filterKeys, parameters_.getEliminationFunction()); +// // The filter summarization should also include any nonlinear factors that involve only the separator variables. +// // Otherwise the smoother will be missing this information +// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) { +// if(factor) { +// NonlinearFactor::const_iterator key = factor->begin(); +// while((key != factor->end()) && (std::binary_search(separatorKeys.begin(), separatorKeys.end(), *key))) { +// ++key; +// } +// if(key == factor->end()) { +// filterSummarization_.push_back(factor); +// } +// } +// } +// +// // Calculate the marginal on the new separator from the smoother factors. This is performed by marginalizing out +// // all of the smoother variables that are not part of the new separator. This smoother summarization will be +// // stored locally for use in the filter +// smootherSummarizationSlots_ = insertFactors( marginalize(graph, values, smootherOrdering, smootherKeys, parameters_.getEliminationFunction()) ); +// } + gttoc(synchronize); } @@ -430,117 +499,301 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac } /* ************************************************************************* */ -void ConcurrentBatchFilter::marginalize(const FastList& keysToMove) { - // In order to marginalize out the selected variables, the factors involved in those variables - // must be identified and removed. Also, the effect of those removed factors on the - // remaining variables needs to be accounted for. This will be done with linear container factors - // from the result of a partial elimination. This function removes the marginalized factors and - // adds the linearized factors back in. +void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { + // In order to move the separator, we need to calculate the marginal information on the new + // separator from all of the factors on the smoother side (both the factors actually in the + // smoother and the ones to be transitioned to the smoother but stored in the filter). + // This is exactly the same operation that is performed by a fixed-lag smoother, calculating + // a marginal factor from the variables outside the smoother window. + // + // However, for the concurrent system, we would like to calculate this marginal in a particular + // way, such that an intermediate term is produced that provides a "shortcut" between the old + // separator (as defined in the smoother) and the new separator. This will allow us to quickly + // update the new separator with changes at the old separator (from the smoother) + + // TODO: This is currently not very efficient: multiple calls to graph.keys(), etc. - // Calculate marginal factors on the remaining variables (after marginalizing 'keyToMove') - // Note: It is assumed the ordering already has these keys first - - // Create the linear factor graph - GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); - - // Calculate the variable index - VariableIndex variableIndex(linearFactorGraph, ordering_.size()); - - // Use the variable Index to mark the factors that will be marginalized - std::set removedFactorSlots; + // Identify all of the new factors to be sent to the smoother (any factor involving keysToMove) + std::vector removedFactorSlots; + VariableIndex variableIndex(*factors_.symbolic(ordering_), theta_.size()); BOOST_FOREACH(Key key, keysToMove) { const FastList& slots = variableIndex[ordering_.at(key)]; - removedFactorSlots.insert(slots.begin(), slots.end()); + removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); + } + // Sort and remove duplicates + std::sort(removedFactorSlots.begin(), removedFactorSlots.end()); + removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end()); + // Remove any linear,marginal factor that made it into the set + BOOST_FOREACH(size_t index, currentSmootherSummarizationSlots_) { + removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end()); } - // Construct an elimination tree to perform sparse elimination - std::vector forest( EliminationForest::Create(linearFactorGraph, variableIndex) ); + // Add these factors to a factor graph + NonlinearFactorGraph removedFactors; + BOOST_FOREACH(size_t slot, removedFactorSlots) { + if(factors_.at(slot)) { + removedFactors.push_back(factors_.at(slot)); + } + } - // This is a tree. Only the top-most nodes/indices need to be eliminated; all of the children will be eliminated automatically - // Find the subset of nodes/keys that must be eliminated - std::set indicesToEliminate; + // Combine the previous shortcut factor with all of the new factors being sent to the smoother + NonlinearFactorGraph graph; + graph.push_back(removedFactors); + graph.push_back(smootherShortcut_); + + // Extract just the values needed for the current marginalization + // and the set all keys + Values values; + BOOST_FOREACH(Key key, graph.keys()) { + values.insert(key, theta_.at(key)); + } + + // Calculate the set of new separator keys (AllAffectedKeys - KeysToMove) + FastSet newSeparatorKeys = removedFactors.keys(); BOOST_FOREACH(Key key, keysToMove) { - indicesToEliminate.insert(ordering_.at(key)); + newSeparatorKeys.erase(key); } + + // Calculate the set of old set of separator keys from the previous summarization factors + FastSet oldSeparatorKeys = previousSmootherSummarization_.keys(); + BOOST_FOREACH(Key key, newSeparatorKeys) { + oldSeparatorKeys.erase(key); + } + + // Calculate the set of OtherKeys = AllKeys - OldSeparator - NewSeparator + FastSet otherKeys(graph.keys()); + BOOST_FOREACH(Key key, oldSeparatorKeys) { + otherKeys.erase(key); + } + BOOST_FOREACH(Key key, newSeparatorKeys) { + otherKeys.erase(key); + } + + // Calculate the ordering: [Others OldSeparator NewSeparator] + typedef std::map OrderingConstraints; + OrderingConstraints constraints; + int group = 0; + if(otherKeys.size() > 0) { + BOOST_FOREACH(Key key, otherKeys) { + constraints[key] = group; + } + ++group; + } + if(oldSeparatorKeys.size() > 0) { + BOOST_FOREACH(Key key, oldSeparatorKeys) { + constraints[key] = group; + } + ++group; + } + if(newSeparatorKeys.size() > 0) { + BOOST_FOREACH(Key key, newSeparatorKeys) { + constraints[key] = group; + } + } + Ordering ordering = *graph.orderingCOLAMDConstrained(values, constraints); + + // Calculate the new shortcut marginal on the OldSeparator + NewSeparator + smootherShortcut_ = marginalize(graph, values, ordering, otherKeys, parameters_.getEliminationFunction()); + + // Combine the old smoother summarization and the new shortcut + graph = NonlinearFactorGraph(); + graph.push_back(previousSmootherSummarization_); + graph.push_back(smootherShortcut_); + + // Extract the values needed for just this graph + values = Values(); + BOOST_FOREACH(Key key, graph.keys()) { + values.insert(key, theta_.at(key)); + } + + // Calculate the ordering: [OldSeparator NewSeparator] + constraints = OrderingConstraints(); + group = 0; + if(oldSeparatorKeys.size() > 0) { + BOOST_FOREACH(Key key, oldSeparatorKeys) { + constraints[key] = group; + } + ++group; + } + if(newSeparatorKeys.size() > 0) { + BOOST_FOREACH(Key key, newSeparatorKeys) { + constraints[key] = group; + } + } + ordering = *graph.orderingCOLAMDConstrained(values, constraints); + + // Calculate the marginal on the new separator + NonlinearFactorGraph marginals = marginalize(graph, values, ordering, oldSeparatorKeys, parameters_.getEliminationFunction()); + + // Remove the previous marginal factors and insert the new marginal factors + removeFactors(currentSmootherSummarizationSlots_); + currentSmootherSummarizationSlots_ = insertFactors(marginals); + + // Update the separatorValues object (should only contain the new separator keys) + separatorValues_.clear(); + BOOST_FOREACH(Key key, marginals.keys()) { + separatorValues_.insert(key, theta_.at(key)); + } + + // Remove the marginalized factors and add them to the smoother cache + smootherFactors_.push_back(removedFactors); + removeFactors(removedFactorSlots); + + // Add the linearization point of the moved variables to the smoother cache BOOST_FOREACH(Key key, keysToMove) { - EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering_.at(key))); + smootherValues_.insert(key, theta_.at(key)); } - // Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables - // Convert the marginal factors into Linear Container Factors - // Add the marginal factor variables to the separator - NonlinearFactorGraph marginalFactors; - BOOST_FOREACH(Index index, indicesToEliminate) { - GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); - 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)); - } - } - } - } - std::vector marginalSlots = insertFactors(marginalFactors); - - - // Cache marginalized variables and factors for later transmission to the smoother - { - // Add the new marginal factors to the list of smootherSeparatorFactors. In essence, we have just moved the separator - smootherSummarizationSlots_.insert(smootherSummarizationSlots_.end(), marginalSlots.begin(), marginalSlots.end()); - - // Move the marginalized factors from the filter to the smoother (holding area) - // Note: Be careful to only move nonlinear factors and not any marginals that may also need to be removed - BOOST_FOREACH(size_t slot, removedFactorSlots) { - std::vector::iterator iter = std::find(smootherSummarizationSlots_.begin(), smootherSummarizationSlots_.end(), slot); - if(iter == smootherSummarizationSlots_.end()) { - // This is a real nonlinear factor. Add it to the smoother factor cache. - smootherFactors_.push_back(factors_.at(slot)); - } else { - // This is a marginal factor that was removed and replaced by a new marginal factor. Remove this slot from the separator factor list. - smootherSummarizationSlots_.erase(iter); - } - } - - // Add the linearization point of the moved variables to the smoother cache - BOOST_FOREACH(Key key, keysToMove) { - smootherValues_.insert(key, theta_.at(key)); - } + // Remove marginalized keys from values (and separator) + BOOST_FOREACH(Key key, keysToMove) { + theta_.erase(key); } - // Remove the marginalized variables and factors from the filter - { - // Remove marginalized factors from the factor graph - std::vector slots(removedFactorSlots.begin(), removedFactorSlots.end()); - removeFactors(slots); - - // Remove marginalized keys from values (and separator) - BOOST_FOREACH(Key key, keysToMove) { - theta_.erase(key); - if(separatorValues_.exists(key)) { - separatorValues_.erase(key); - } - } - - // Permute the ordering such that the removed keys are at the end. - // This is a prerequisite for removing them from several structures - std::vector toBack; - BOOST_FOREACH(Key key, keysToMove) { - toBack.push_back(ordering_.at(key)); - } - Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); - ordering_.permuteInPlace(forwardPermutation); - delta_.permuteInPlace(forwardPermutation); - - // Remove marginalized keys from the ordering and delta - for(size_t i = 0; i < keysToMove.size(); ++i) { - ordering_.pop_back(); - delta_.pop_back(); - } + // Permute the ordering such that the removed keys are at the end. + // This is a prerequisite for removing them from several structures + std::vector toBack; + BOOST_FOREACH(Key key, keysToMove) { + toBack.push_back(ordering_.at(key)); } + Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); + ordering_.permuteInPlace(forwardPermutation); + delta_.permuteInPlace(forwardPermutation); + + // Remove marginalized keys from the ordering and delta + for(size_t i = 0; i < keysToMove.size(); ++i) { + ordering_.pop_back(); + delta_.pop_back(); + } + + +// // Calculate marginal factors on the remaining variables (after marginalizing 'keyToMove') +// // Note: It is assumed the ordering already has these keys first +// +// // text +// NonlinearFactorGraph marginalFactors = marginalize(factors_, theta_, ordering_, keysToMove, parameters_.getEliminationFunction()); +// +// // text +// BOOST_FOREACH(const NonlinearFactor::shared_ptr& marginalFactor, marginalFactors) { +// BOOST_FOREACH(Key key, *marginalFactor) { +// if(!separatorValues_.exists(key)) { +// separatorValues_.insert(key, theta_.at(key)); +// } +// } +// } +// std::vector marginalSlots = insertFactors(marginalFactors); +// +// // text +// // Use the variable Index to mark the factors that will be marginalized +// std::set removedFactorSlots; +// BOOST_FOREACH(Key key, keysToMove) { +// const FastList& slots = variableIndex[ordering_.at(key)]; +// removedFactorSlots.insert(slots.begin(), slots.end()); +// } +// +// +// +// +// // Create the linear factor graph +// GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); +// +// // Calculate the variable index +// VariableIndex variableIndex(linearFactorGraph, ordering_.size()); +// +// // Use the variable Index to mark the factors that will be marginalized +// std::set removedFactorSlots; +// BOOST_FOREACH(Key key, keysToMove) { +// const FastList& slots = variableIndex[ordering_.at(key)]; +// removedFactorSlots.insert(slots.begin(), slots.end()); +// } +// +// // Construct an elimination tree to perform sparse elimination +// std::vector forest( EliminationForest::Create(linearFactorGraph, variableIndex) ); +// +// // This is a tree. Only the top-most nodes/indices need to be eliminated; all of the children will be eliminated automatically +// // Find the subset of nodes/keys that must be eliminated +// std::set indicesToEliminate; +// BOOST_FOREACH(Key key, keysToMove) { +// indicesToEliminate.insert(ordering_.at(key)); +// } +// BOOST_FOREACH(Key key, keysToMove) { +// EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering_.at(key))); +// } +// +// // Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables +// // Convert the marginal factors into Linear Container Factors +// // Add the marginal factor variables to the separator +// NonlinearFactorGraph marginalFactors; +// BOOST_FOREACH(Index index, indicesToEliminate) { +// GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); +// 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)); +// } +// } +// } +// } +// std::vector marginalSlots = insertFactors(marginalFactors); +// +// +// // Cache marginalized variables and factors for later transmission to the smoother +// { +// // Add the new marginal factors to the list of smootherSeparatorFactors. In essence, we have just moved the separator +// smootherSummarizationSlots_.insert(smootherSummarizationSlots_.end(), marginalSlots.begin(), marginalSlots.end()); +// +// // Move the marginalized factors from the filter to the smoother (holding area) +// // Note: Be careful to only move nonlinear factors and not any marginals that may also need to be removed +// BOOST_FOREACH(size_t slot, removedFactorSlots) { +// std::vector::iterator iter = std::find(smootherSummarizationSlots_.begin(), smootherSummarizationSlots_.end(), slot); +// if(iter == smootherSummarizationSlots_.end()) { +// // This is a real nonlinear factor. Add it to the smoother factor cache. +// smootherFactors_.push_back(factors_.at(slot)); +// } else { +// // This is a marginal factor that was removed and replaced by a new marginal factor. Remove this slot from the smoother summarization list. +// smootherSummarizationSlots_.erase(iter); +// } +// } +// +// // Add the linearization point of the moved variables to the smoother cache +// BOOST_FOREACH(Key key, keysToMove) { +// smootherValues_.insert(key, theta_.at(key)); +// } +// } +// +// // Remove the marginalized variables and factors from the filter +// { +// // Remove marginalized factors from the factor graph +// std::vector slots(removedFactorSlots.begin(), removedFactorSlots.end()); +// removeFactors(slots); +// +// // Remove marginalized keys from values (and separator) +// BOOST_FOREACH(Key key, keysToMove) { +// theta_.erase(key); +// if(separatorValues_.exists(key)) { +// separatorValues_.erase(key); +// } +// } +// +// // Permute the ordering such that the removed keys are at the end. +// // This is a prerequisite for removing them from several structures +// std::vector toBack; +// BOOST_FOREACH(Key key, keysToMove) { +// toBack.push_back(ordering_.at(key)); +// } +// Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); +// ordering_.permuteInPlace(forwardPermutation); +// delta_.permuteInPlace(forwardPermutation); +// +// // Remove marginalized keys from the ordering and delta +// for(size_t i = 0; i < keysToMove.size(); ++i) { +// ordering_.pop_back(); +// delta_.pop_back(); +// } +// } } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 9a7690786..7feee4d82 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -114,7 +114,7 @@ public: * @param newTheta Initialization points for new variables to be added to the filter * You must include here all new variables occurring in newFactors that were not already * in the filter. - * @param keysToMove An optional set of keys to remove from the filter and + * @param keysToMove An optional set of keys to move from the filter to the smoother */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const boost::optional >& keysToMove = boost::none); @@ -129,7 +129,11 @@ protected: VectorValues delta_; ///< The current set of linear deltas from the linearization point std::queue availableSlots_; ///< The set of available factor graph slots caused by deleting factors Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization. - std::vector smootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization factors + std::vector currentSmootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator + + // Storage for information from the Smoother + NonlinearFactorGraph previousSmootherSummarization_; ///< The smoother summarization on the old separator sent by the smoother during the last synchronization + NonlinearFactorGraph smootherShortcut_; ///< A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update) // Storage for information to be sent to the smoother NonlinearFactorGraph filterSummarization_; ///< A temporary holding place for calculated filter summarization factors to be sent to the smoother @@ -193,15 +197,17 @@ private: /** Use colamd to update into an efficient ordering */ void reorder(const boost::optional >& keysToMove = boost::none); + /** Marginalize out the set of requested variables from the filter, caching them for the smoother + * This effectively moves the separator. + * + * @param keysToMove The set of keys to move from the filter to the smoother + */ + void moveSeparator(const FastList& keysToMove); + /** Use a modified version of L-M to update the linearization point and delta */ static Result optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering, VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters); - /** Marginalize out the set of requested variables from the filter, caching them for the smoother - * This effectively moves the separator. - */ - void marginalize(const FastList& keysToMove); - /** Marginalize out the set of requested variables from the filter, caching them for the smoother * This effectively moves the separator. */ From 8e26da7396bb5286e77037181327dfe409049848 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Tue, 21 May 2013 21:07:45 +0000 Subject: [PATCH 03/47] Added matlab version of the Concurrent Filtering and Smoothing example --- ...ConcurrentFilteringAndSmoothingExample.cpp | 2 +- .../ConcurrentFilteringAndSmoothingExample.m | 131 ++++++++++++++++++ 2 files changed, 132 insertions(+), 1 deletion(-) create mode 100644 matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m diff --git a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp index 002d24ad4..65fc681c4 100644 --- a/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp +++ b/gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp @@ -118,7 +118,7 @@ int main(int argc, char** argv) { newFactors.add(BetweenFactor(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); // Unlike the fixed-lag versions, the concurrent filter implementation - // requires the user to supply the specify which keys to marginalize + // requires the user to supply the specify which keys to move to the smoother FastList oldKeys; if(time >= lag+deltaT) { oldKeys.push_back(1000 * (time-lag-deltaT)); diff --git a/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m b/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m new file mode 100644 index 000000000..bb6e63dc6 --- /dev/null +++ b/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m @@ -0,0 +1,131 @@ +% ---------------------------------------------------------------------------- +% +% GTSAM Copyright 2010, Georgia Tech Research Corporation, +% Atlanta, Georgia 30332-0415 +% All Rights Reserved +% Authors: Frank Dellaert, et al. (see THANKS for the full author list) +% +% See LICENSE for the license information +% +% -------------------------------------------------------------------------- + +% @file ConcurrentFilteringAndSmoothingExample.m +% @brief Demonstration of the concurrent filtering and smoothing architecture using +% a planar robot example and multiple odometry-like sensors +% @author Stephen Williams + +% A simple 2D pose slam example with multiple odometry-like measurements +% - The robot initially faces along the X axis (horizontal, to the right in 2D) +% - The robot moves forward at 2m/s +% - We have measurements between each pose from multiple odometry sensors + +clear all; +import gtsam.*; + +% Define the smoother lag (in seconds) +lag = 2.0; + +% Create a Concurrent Filter and Smoother +concurrentFilter = ConcurrentBatchFilter; +concurrentSmoother = ConcurrentBatchSmoother; + +%% Create containers to store the factors and linearization points that +% will be sent to the smoothers +newFactors = NonlinearFactorGraph; +newValues = Values; + +%% Create a prior on the first pose, placing it at the origin +priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin +priorNoise = noiseModel.Diagonal.Sigmas([0.3 ; 0.3 ; 0.1]); +priorKey = uint64(0); +newFactors.add(PriorFactorPose2(priorKey, priorMean, priorNoise)); +newValues.insert(priorKey, priorMean); % Initialize the first pose at the mean of the prior + +%% Insert the prior factor into the filter +concurrentFilter.update(newFactors, newValues); + +%% Now, loop through several time steps, creating factors from different "sensors" +% and adding them to the fixed-lag smoothers +deltaT = 0.25; +for time = deltaT : deltaT : 10.0 + + %% Initialize factor and values for this loop iteration + newFactors = NonlinearFactorGraph; + newValues = Values; + + %% Define the keys related to this timestamp + previousKey = uint64(1000 * (time-deltaT)); + currentKey = uint64(1000 * (time)); + + %% Add a guess for this pose to the new values + % Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s] + % {This is not a particularly good way to guess, but this is just an example} + currentPose = Pose2(time * 2.0, 0.0, 0.0); + newValues.insert(currentKey, currentPose); + + %% Add odometry factors from two different sources with different error stats + odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); + odometryNoise1 = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]); + newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); + + odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); + odometryNoise2 = noiseModel.Isotropic.Sigma(3, 0.05); + newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); + + %% Unlike the fixed-lag versions, the concurrent filter implementation + % requires the user to supply the specify which keys to move to the smoother + oldKeys = KeyList; + if time >= lag+deltaT + oldKeys.push_back(uint64(1000 * (time-lag-deltaT))); + end + + %% Update the various inference engines + concurrentFilter.update(newFactors, newValues, oldKeys); + + %% Add a loop closure to the smoother at a particular time + if time == 8.0 + % Now lets create a "loop closure" factor between some poses + loopKey1 = uint64(1000 * (0.0)); + loopKey2 = uint64(1000 * (5.0)); + loopMeasurement = Pose2(9.5, 1.00, 0.00); + loopNoise = noiseModel.Diagonal.Sigmas([0.5; 0.5; 0.25]); + loopFactor = BetweenFactorPose2(loopKey1, loopKey2, loopMeasurement, loopNoise); + % The state at 5.0s should have been transferred to the concurrent smoother at this point. Add the loop closure. + smootherFactors = NonlinearFactorGraph; + smootherFactors.push_back(loopFactor); + concurrentSmoother.update(smootherFactors, Values()); + end + + %% Manually synchronize the Concurrent Filter and Smoother every 1.0 s + if mod(time, 1.0) < 0.01 + % Synchronize the Filter and Smoother + concurrentSmoother.update(); + synchronize(concurrentFilter, concurrentSmoother); + end + + %% Print the filter optimized poses + fprintf(1, 'Timestamp = %5.3f\n', time); + filterResult = concurrentFilter.calculateEstimate; + filterResult.at(currentKey).print('Concurrent Estimate: '); + + %% Plot Covariance Ellipses + cla; + hold on + filterMarginals = Marginals(concurrentFilter.getFactors, filterResult); + plot2DTrajectory(filterResult, 'k*-', filterMarginals); + + smootherGraph = concurrentSmoother.getFactors; + if smootherGraph.size > 0 + smootherResult = concurrentSmoother.calculateEstimate; + smootherMarginals = Marginals(smootherGraph, smootherResult); + plot2DTrajectory(smootherResult, 'r*-', smootherMarginals); + end + + axis equal + axis tight + view(2) +pause +end + + + From 75803f02294e2136e8af2f2a7f11455e022731fb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 21 May 2013 21:22:55 +0000 Subject: [PATCH 04/47] smart indent, and change push_back to add --- .../ConcurrentFilteringAndSmoothingExample.m | 169 +++++++++--------- 1 file changed, 85 insertions(+), 84 deletions(-) diff --git a/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m b/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m index bb6e63dc6..d1d225545 100644 --- a/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m +++ b/matlab/unstable_examples/ConcurrentFilteringAndSmoothingExample.m @@ -1,14 +1,14 @@ % ---------------------------------------------------------------------------- -% -% GTSAM Copyright 2010, Georgia Tech Research Corporation, +% +% GTSAM Copyright 2010, Georgia Tech Research Corporation, % Atlanta, Georgia 30332-0415 % All Rights Reserved % Authors: Frank Dellaert, et al. (see THANKS for the full author list) -% +% % See LICENSE for the license information -% +% % -------------------------------------------------------------------------- - + % @file ConcurrentFilteringAndSmoothingExample.m % @brief Demonstration of the concurrent filtering and smoothing architecture using % a planar robot example and multiple odometry-like sensors @@ -19,6 +19,7 @@ % - The robot moves forward at 2m/s % - We have measurements between each pose from multiple odometry sensors +clear all; clear all; import gtsam.*; @@ -33,7 +34,7 @@ concurrentSmoother = ConcurrentBatchSmoother; % will be sent to the smoothers newFactors = NonlinearFactorGraph; newValues = Values; - + %% Create a prior on the first pose, placing it at the origin priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin priorNoise = noiseModel.Diagonal.Sigmas([0.3 ; 0.3 ; 0.1]); @@ -48,84 +49,84 @@ concurrentFilter.update(newFactors, newValues); % and adding them to the fixed-lag smoothers deltaT = 0.25; for time = deltaT : deltaT : 10.0 - - %% Initialize factor and values for this loop iteration - newFactors = NonlinearFactorGraph; - newValues = Values; - - %% Define the keys related to this timestamp - previousKey = uint64(1000 * (time-deltaT)); - currentKey = uint64(1000 * (time)); - - %% Add a guess for this pose to the new values - % Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s] - % {This is not a particularly good way to guess, but this is just an example} - currentPose = Pose2(time * 2.0, 0.0, 0.0); - newValues.insert(currentKey, currentPose); - - %% Add odometry factors from two different sources with different error stats - odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); - odometryNoise1 = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]); - newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); - - odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); - odometryNoise2 = noiseModel.Isotropic.Sigma(3, 0.05); - newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); - - %% Unlike the fixed-lag versions, the concurrent filter implementation - % requires the user to supply the specify which keys to move to the smoother - oldKeys = KeyList; - if time >= lag+deltaT - oldKeys.push_back(uint64(1000 * (time-lag-deltaT))); - end - - %% Update the various inference engines - concurrentFilter.update(newFactors, newValues, oldKeys); - - %% Add a loop closure to the smoother at a particular time - if time == 8.0 - % Now lets create a "loop closure" factor between some poses - loopKey1 = uint64(1000 * (0.0)); - loopKey2 = uint64(1000 * (5.0)); - loopMeasurement = Pose2(9.5, 1.00, 0.00); - loopNoise = noiseModel.Diagonal.Sigmas([0.5; 0.5; 0.25]); - loopFactor = BetweenFactorPose2(loopKey1, loopKey2, loopMeasurement, loopNoise); - % The state at 5.0s should have been transferred to the concurrent smoother at this point. Add the loop closure. - smootherFactors = NonlinearFactorGraph; - smootherFactors.push_back(loopFactor); - concurrentSmoother.update(smootherFactors, Values()); - end - - %% Manually synchronize the Concurrent Filter and Smoother every 1.0 s - if mod(time, 1.0) < 0.01 - % Synchronize the Filter and Smoother - concurrentSmoother.update(); - synchronize(concurrentFilter, concurrentSmoother); - end - - %% Print the filter optimized poses - fprintf(1, 'Timestamp = %5.3f\n', time); - filterResult = concurrentFilter.calculateEstimate; - filterResult.at(currentKey).print('Concurrent Estimate: '); - - %% Plot Covariance Ellipses - cla; - hold on - filterMarginals = Marginals(concurrentFilter.getFactors, filterResult); - plot2DTrajectory(filterResult, 'k*-', filterMarginals); - - smootherGraph = concurrentSmoother.getFactors; - if smootherGraph.size > 0 - smootherResult = concurrentSmoother.calculateEstimate; - smootherMarginals = Marginals(smootherGraph, smootherResult); - plot2DTrajectory(smootherResult, 'r*-', smootherMarginals); - end - - axis equal - axis tight - view(2) -pause -end + %% Initialize factor and values for this loop iteration + newFactors = NonlinearFactorGraph; + newValues = Values; + + %% Define the keys related to this timestamp + previousKey = uint64(1000 * (time-deltaT)); + currentKey = uint64(1000 * (time)); + + %% Add a guess for this pose to the new values + % Since the robot moves forward at 2 m/s, then the position is simply: time[s]*2.0[m/s] + % {This is not a particularly good way to guess, but this is just an example} + currentPose = Pose2(time * 2.0, 0.0, 0.0); + newValues.insert(currentKey, currentPose); + + %% Add odometry factors from two different sources with different error stats + odometryMeasurement1 = Pose2(0.61, -0.08, 0.02); + odometryNoise1 = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.05]); + newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement1, odometryNoise1)); + + odometryMeasurement2 = Pose2(0.47, 0.03, 0.01); + odometryNoise2 = noiseModel.Isotropic.Sigma(3, 0.05); + newFactors.add(BetweenFactorPose2(previousKey, currentKey, odometryMeasurement2, odometryNoise2)); + + %% Unlike the fixed-lag versions, the concurrent filter implementation + % requires the user to supply the specify which keys to move to the smoother + oldKeys = KeyList; + if time >= lag+deltaT + oldKeys.push_back(uint64(1000 * (time-lag-deltaT))); + end + + %% Update the various inference engines + concurrentFilter.update(newFactors, newValues, oldKeys); + + %% Add a loop closure to the smoother at a particular time + if time == 8.0 + % Now lets create a "loop closure" factor between some poses + loopKey1 = uint64(1000 * (0.0)); + loopKey2 = uint64(1000 * (5.0)); + loopMeasurement = Pose2(9.5, 1.00, 0.00); + loopNoise = noiseModel.Diagonal.Sigmas([0.5; 0.5; 0.25]); + loopFactor = BetweenFactorPose2(loopKey1, loopKey2, loopMeasurement, loopNoise); + % The state at 5.0s should have been transferred to the concurrent smoother at this point. Add the loop closure. + smootherFactors = NonlinearFactorGraph; + smootherFactors.add(loopFactor); + concurrentSmoother.update(smootherFactors, Values()); + end + + %% Manually synchronize the Concurrent Filter and Smoother every 1.0 s + if mod(time, 1.0) < 0.01 + % Synchronize the Filter and Smoother + concurrentSmoother.update(); + synchronize(concurrentFilter, concurrentSmoother); + end + + %% Print the filter optimized poses + fprintf(1, 'Timestamp = %5.3f\n', time); + filterResult = concurrentFilter.calculateEstimate; + filterResult.at(currentKey).print('Concurrent Estimate: '); + + %% Plot Covariance Ellipses + cla; + hold on + filterMarginals = Marginals(concurrentFilter.getFactors, filterResult); + plot2DTrajectory(filterResult, 'k*-', filterMarginals); + + smootherGraph = concurrentSmoother.getFactors; + if smootherGraph.size > 0 + smootherResult = concurrentSmoother.calculateEstimate; + smootherMarginals = Marginals(smootherGraph, smootherResult); + plot2DTrajectory(smootherResult, 'r*-', smootherMarginals); + end + + axis equal + axis tight + view(2) + pause(0.01) +end + From 1e5f9c742d49d99c23fa5b582e3c02754b19758a Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Wed, 22 May 2013 00:05:03 +0000 Subject: [PATCH 05/47] Revert "Modified Concurrent Filter to calculate marginals using a "shortcut" that allows constant-time updates during synchronization. Still need to test implementation." This reverts commit f24a4f4668006cfe9a3eeb1658b7df03c74490d5. --- .../nonlinear/ConcurrentBatchFilter.cpp | 621 ++++++------------ .../nonlinear/ConcurrentBatchFilter.h | 20 +- 2 files changed, 191 insertions(+), 450 deletions(-) diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index d8dcd6c14..1e1e39e10 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -85,11 +85,11 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFacto } gttoc(optimize); - gttic(move_separator); + gttic(marginalize); if(keysToMove && keysToMove->size() > 0){ - moveSeparator(*keysToMove); + marginalize(*keysToMove); } - gttoc(move_separator); + gttoc(marginalize); gttoc(update); @@ -109,169 +109,100 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFa gttic(synchronize); - // Update the smoother summarization on the old separator - previousSmootherSummarization_ = summarizedFactors; + // Remove the previous smoother summarization + removeFactors(smootherSummarizationSlots_); - // Use the shortcut to calculate an updated marginal on the current separator - { - // Combine the new smoother summarization and the existing shortcut - NonlinearFactorGraph graph; - graph.push_back(previousSmootherSummarization_); - graph.push_back(smootherShortcut_); + // Create a factor graph containing the new smoother summarization, the factors to be sent to the smoother, + // and all of the filter factors. This is the set of factors on the filter side since the smoother started + // its previous update cycle. + NonlinearFactorGraph graph; + graph.push_back(factors_); + graph.push_back(smootherFactors_); + graph.push_back(summarizedFactors); + Values values; + values.insert(theta_); + values.insert(smootherValues_); + values.update(separatorValues); // ensure the smoother summarized factors are linearized around the values in the smoother - // Extract the values needed for just this graph - Values values; - BOOST_FOREACH(Key key, graph.keys()) { - values.insert(key, theta_.at(key)); - } - - // Calculate the ordering: [OldSeparator NewSeparator] - // And determine the set of variables to be marginalized out - std::map constraints; - std::set marginalizeKeys; - int group = 0; - if(values.size() > 0) { - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, values) { - constraints[key_value.key] = group; - marginalizeKeys.insert(key_value.key); - } - ++group; - } - if(separatorValues_.size() > 0) { - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - constraints[key_value.key] = group; - marginalizeKeys.erase(key_value.key); - } - } - Ordering ordering = *graph.orderingCOLAMDConstrained(values, constraints); - - // Calculate the marginal on the new separator (from the smoother side) - NonlinearFactorGraph marginals = marginalize(graph, values, ordering, marginalizeKeys, parameters_.getEliminationFunction()); - - // Remove old summarization and insert new - removeFactors(currentSmootherSummarizationSlots_); - currentSmootherSummarizationSlots_ = insertFactors(marginals); - } - - // Calculate the marginal on the new separator from the filter factors - // Note: This could also be done during each filter update so it would simply be available - { - // Calculate an ordering that places the new separator at the root - // And determine the set of variables to be marginalized out - std::map constraints; - std::set marginalizeKeys; - int group = 0; - if(theta_.size() > 0) { + if(factors_.size() > 0) { + // Perform an optional optimization on the to-be-sent-to-the-smoother factors + if(relin_) { + // Create ordering and delta + Ordering ordering = *graph.orderingCOLAMD(values); + VectorValues delta = values.zeroVectors(ordering); + // Optimize this graph using a modified version of L-M + optimize(graph, values, ordering, delta, separatorValues, parameters_); + // Update filter theta and delta BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { - constraints[key_value.key] = group; - marginalizeKeys.insert(key_value.key); + theta_.update(key_value.key, values.at(key_value.key)); + delta_.at(ordering_.at(key_value.key)) = delta.at(ordering.at(key_value.key)); } - ++group; - } - if(separatorValues_.size() > 0) { + // Update the fixed linearization points (since they just changed) BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { - constraints[key_value.key] = group; - marginalizeKeys.erase(key_value.key); + separatorValues_.update(key_value.key, values.at(key_value.key)); } } - Ordering ordering = *factors_.orderingCOLAMDConstrained(theta_, constraints); - // Calculate the marginal on the new separator (from the filter side) - filterSummarization_ = marginalize(factors_, theta_, ordering, marginalizeKeys, parameters_.getEliminationFunction()); + // Create separate ordering constraints that force either the filter keys or the smoother keys to the front + typedef std::map OrderingConstraints; + OrderingConstraints filterConstraints; + OrderingConstraints smootherConstraints; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { /// the filter keys + filterConstraints[key_value.key] = 0; + smootherConstraints[key_value.key] = 1; + } + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { /// the smoother keys + filterConstraints[key_value.key] = 1; + smootherConstraints[key_value.key] = 0; + } + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { /// the *new* separator keys + filterConstraints[key_value.key] = 2; + smootherConstraints[key_value.key] = 2; + } + + // Generate separate orderings that place the filter keys or the smoother keys first + // TODO: This is convenient, but it recalculates the variable index each time + Ordering filterOrdering = *graph.orderingCOLAMDConstrained(values, filterConstraints); + Ordering smootherOrdering = *graph.orderingCOLAMDConstrained(values, smootherConstraints); + + // Extract the set of filter keys and smoother keys + std::set filterKeys; + std::set separatorKeys; + std::set smootherKeys; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { + filterKeys.insert(key_value.key); + } + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + separatorKeys.insert(key_value.key); + filterKeys.erase(key_value.key); + } + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { + smootherKeys.insert(key_value.key); + } + + // Calculate the marginal on the new separator from the filter factors. This is performed by marginalizing out + // all of the filter variables that are not part of the new separator. This filter summarization will then be + // sent to the smoother. + filterSummarization_ = marginalize(graph, values, filterOrdering, filterKeys, parameters_.getEliminationFunction()); + // The filter summarization should also include any nonlinear factors that involve only the separator variables. + // Otherwise the smoother will be missing this information + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) { + if(factor) { + NonlinearFactor::const_iterator key = factor->begin(); + while((key != factor->end()) && (std::binary_search(separatorKeys.begin(), separatorKeys.end(), *key))) { + ++key; + } + if(key == factor->end()) { + filterSummarization_.push_back(factor); + } + } + } + + // Calculate the marginal on the new separator from the smoother factors. This is performed by marginalizing out + // all of the smoother variables that are not part of the new separator. This smoother summarization will be + // stored locally for use in the filter + smootherSummarizationSlots_ = insertFactors( marginalize(graph, values, smootherOrdering, smootherKeys, parameters_.getEliminationFunction()) ); } - -// // Create a factor graph containing the new smoother summarization, the factors to be sent to the smoother, -// // and all of the filter factors. This is the set of factors on the filter side since the smoother started -// // its previous update cycle. -// NonlinearFactorGraph graph; -// graph.push_back(factors_); -// graph.push_back(smootherFactors_); -// graph.push_back(summarizedFactors); -// Values values; -// values.insert(theta_); -// values.insert(smootherValues_); -// values.update(separatorValues); // ensure the smoother summarized factors are linearized around the values in the smoother -// -// if(factors_.size() > 0) { -// // Perform an optional optimization on the to-be-sent-to-the-smoother factors -// if(true) { -// // Create ordering and delta -// Ordering ordering = *graph.orderingCOLAMD(values); -// VectorValues delta = values.zeroVectors(ordering); -// // Optimize this graph using a modified version of L-M -// optimize(graph, values, ordering, delta, separatorValues, parameters_); -// // Update filter theta and delta -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { -// theta_.update(key_value.key, values.at(key_value.key)); -// delta_.at(ordering_.at(key_value.key)) = delta.at(ordering.at(key_value.key)); -// } -// // Update the fixed linearization points (since they just changed) -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { -// separatorValues_.update(key_value.key, values.at(key_value.key)); -// } -// } -// -// // Create separate ordering constraints that force either the filter keys or the smoother keys to the front -// typedef std::map OrderingConstraints; -// OrderingConstraints filterConstraints; -// OrderingConstraints smootherConstraints; -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { /// the filter keys -// filterConstraints[key_value.key] = 0; -// smootherConstraints[key_value.key] = 1; -// } -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { /// the smoother keys -// filterConstraints[key_value.key] = 1; -// smootherConstraints[key_value.key] = 0; -// } -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { /// the *new* separator keys -// filterConstraints[key_value.key] = 2; -// smootherConstraints[key_value.key] = 2; -// } -// -// // Generate separate orderings that place the filter keys or the smoother keys first -// // TODO: This is convenient, but it recalculates the variable index each time -// Ordering filterOrdering = *graph.orderingCOLAMDConstrained(values, filterConstraints); -// Ordering smootherOrdering = *graph.orderingCOLAMDConstrained(values, smootherConstraints); -// -// // Extract the set of filter keys and smoother keys -// std::set filterKeys; -// std::set separatorKeys; -// std::set smootherKeys; -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, theta_) { -// filterKeys.insert(key_value.key); -// } -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { -// separatorKeys.insert(key_value.key); -// filterKeys.erase(key_value.key); -// } -// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { -// smootherKeys.insert(key_value.key); -// } -// -// // Calculate the marginal on the new separator from the filter factors. This is performed by marginalizing out -// // all of the filter variables that are not part of the new separator. This filter summarization will then be -// // sent to the smoother. -// filterSummarization_ = marginalize(graph, values, filterOrdering, filterKeys, parameters_.getEliminationFunction()); -// // The filter summarization should also include any nonlinear factors that involve only the separator variables. -// // Otherwise the smoother will be missing this information -// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors_) { -// if(factor) { -// NonlinearFactor::const_iterator key = factor->begin(); -// while((key != factor->end()) && (std::binary_search(separatorKeys.begin(), separatorKeys.end(), *key))) { -// ++key; -// } -// if(key == factor->end()) { -// filterSummarization_.push_back(factor); -// } -// } -// } -// -// // Calculate the marginal on the new separator from the smoother factors. This is performed by marginalizing out -// // all of the smoother variables that are not part of the new separator. This smoother summarization will be -// // stored locally for use in the filter -// smootherSummarizationSlots_ = insertFactors( marginalize(graph, values, smootherOrdering, smootherKeys, parameters_.getEliminationFunction()) ); -// } - gttoc(synchronize); } @@ -499,301 +430,117 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac } /* ************************************************************************* */ -void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { - // In order to move the separator, we need to calculate the marginal information on the new - // separator from all of the factors on the smoother side (both the factors actually in the - // smoother and the ones to be transitioned to the smoother but stored in the filter). - // This is exactly the same operation that is performed by a fixed-lag smoother, calculating - // a marginal factor from the variables outside the smoother window. - // - // However, for the concurrent system, we would like to calculate this marginal in a particular - // way, such that an intermediate term is produced that provides a "shortcut" between the old - // separator (as defined in the smoother) and the new separator. This will allow us to quickly - // update the new separator with changes at the old separator (from the smoother) - - // TODO: This is currently not very efficient: multiple calls to graph.keys(), etc. +void ConcurrentBatchFilter::marginalize(const FastList& keysToMove) { + // In order to marginalize out the selected variables, the factors involved in those variables + // must be identified and removed. Also, the effect of those removed factors on the + // remaining variables needs to be accounted for. This will be done with linear container factors + // from the result of a partial elimination. This function removes the marginalized factors and + // adds the linearized factors back in. - // Identify all of the new factors to be sent to the smoother (any factor involving keysToMove) - std::vector removedFactorSlots; - VariableIndex variableIndex(*factors_.symbolic(ordering_), theta_.size()); + // Calculate marginal factors on the remaining variables (after marginalizing 'keyToMove') + // Note: It is assumed the ordering already has these keys first + + // Create the linear factor graph + GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); + + // Calculate the variable index + VariableIndex variableIndex(linearFactorGraph, ordering_.size()); + + // Use the variable Index to mark the factors that will be marginalized + std::set removedFactorSlots; BOOST_FOREACH(Key key, keysToMove) { const FastList& slots = variableIndex[ordering_.at(key)]; - removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); - } - // Sort and remove duplicates - std::sort(removedFactorSlots.begin(), removedFactorSlots.end()); - removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end()); - // Remove any linear,marginal factor that made it into the set - BOOST_FOREACH(size_t index, currentSmootherSummarizationSlots_) { - removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end()); + removedFactorSlots.insert(slots.begin(), slots.end()); } - // Add these factors to a factor graph - NonlinearFactorGraph removedFactors; - BOOST_FOREACH(size_t slot, removedFactorSlots) { - if(factors_.at(slot)) { - removedFactors.push_back(factors_.at(slot)); - } - } + // Construct an elimination tree to perform sparse elimination + std::vector forest( EliminationForest::Create(linearFactorGraph, variableIndex) ); - // Combine the previous shortcut factor with all of the new factors being sent to the smoother - NonlinearFactorGraph graph; - graph.push_back(removedFactors); - graph.push_back(smootherShortcut_); - - // Extract just the values needed for the current marginalization - // and the set all keys - Values values; - BOOST_FOREACH(Key key, graph.keys()) { - values.insert(key, theta_.at(key)); - } - - // Calculate the set of new separator keys (AllAffectedKeys - KeysToMove) - FastSet newSeparatorKeys = removedFactors.keys(); + // This is a tree. Only the top-most nodes/indices need to be eliminated; all of the children will be eliminated automatically + // Find the subset of nodes/keys that must be eliminated + std::set indicesToEliminate; BOOST_FOREACH(Key key, keysToMove) { - newSeparatorKeys.erase(key); + indicesToEliminate.insert(ordering_.at(key)); } - - // Calculate the set of old set of separator keys from the previous summarization factors - FastSet oldSeparatorKeys = previousSmootherSummarization_.keys(); - BOOST_FOREACH(Key key, newSeparatorKeys) { - oldSeparatorKeys.erase(key); - } - - // Calculate the set of OtherKeys = AllKeys - OldSeparator - NewSeparator - FastSet otherKeys(graph.keys()); - BOOST_FOREACH(Key key, oldSeparatorKeys) { - otherKeys.erase(key); - } - BOOST_FOREACH(Key key, newSeparatorKeys) { - otherKeys.erase(key); - } - - // Calculate the ordering: [Others OldSeparator NewSeparator] - typedef std::map OrderingConstraints; - OrderingConstraints constraints; - int group = 0; - if(otherKeys.size() > 0) { - BOOST_FOREACH(Key key, otherKeys) { - constraints[key] = group; - } - ++group; - } - if(oldSeparatorKeys.size() > 0) { - BOOST_FOREACH(Key key, oldSeparatorKeys) { - constraints[key] = group; - } - ++group; - } - if(newSeparatorKeys.size() > 0) { - BOOST_FOREACH(Key key, newSeparatorKeys) { - constraints[key] = group; - } - } - Ordering ordering = *graph.orderingCOLAMDConstrained(values, constraints); - - // Calculate the new shortcut marginal on the OldSeparator + NewSeparator - smootherShortcut_ = marginalize(graph, values, ordering, otherKeys, parameters_.getEliminationFunction()); - - // Combine the old smoother summarization and the new shortcut - graph = NonlinearFactorGraph(); - graph.push_back(previousSmootherSummarization_); - graph.push_back(smootherShortcut_); - - // Extract the values needed for just this graph - values = Values(); - BOOST_FOREACH(Key key, graph.keys()) { - values.insert(key, theta_.at(key)); - } - - // Calculate the ordering: [OldSeparator NewSeparator] - constraints = OrderingConstraints(); - group = 0; - if(oldSeparatorKeys.size() > 0) { - BOOST_FOREACH(Key key, oldSeparatorKeys) { - constraints[key] = group; - } - ++group; - } - if(newSeparatorKeys.size() > 0) { - BOOST_FOREACH(Key key, newSeparatorKeys) { - constraints[key] = group; - } - } - ordering = *graph.orderingCOLAMDConstrained(values, constraints); - - // Calculate the marginal on the new separator - NonlinearFactorGraph marginals = marginalize(graph, values, ordering, oldSeparatorKeys, parameters_.getEliminationFunction()); - - // Remove the previous marginal factors and insert the new marginal factors - removeFactors(currentSmootherSummarizationSlots_); - currentSmootherSummarizationSlots_ = insertFactors(marginals); - - // Update the separatorValues object (should only contain the new separator keys) - separatorValues_.clear(); - BOOST_FOREACH(Key key, marginals.keys()) { - separatorValues_.insert(key, theta_.at(key)); - } - - // Remove the marginalized factors and add them to the smoother cache - smootherFactors_.push_back(removedFactors); - removeFactors(removedFactorSlots); - - // Add the linearization point of the moved variables to the smoother cache BOOST_FOREACH(Key key, keysToMove) { - smootherValues_.insert(key, theta_.at(key)); + EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering_.at(key))); } - // Remove marginalized keys from values (and separator) - BOOST_FOREACH(Key key, keysToMove) { - theta_.erase(key); + // Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables + // Convert the marginal factors into Linear Container Factors + // Add the marginal factor variables to the separator + NonlinearFactorGraph marginalFactors; + BOOST_FOREACH(Index index, indicesToEliminate) { + GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); + 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)); + } + } + } + } + std::vector marginalSlots = insertFactors(marginalFactors); + + + // Cache marginalized variables and factors for later transmission to the smoother + { + // Add the new marginal factors to the list of smootherSeparatorFactors. In essence, we have just moved the separator + smootherSummarizationSlots_.insert(smootherSummarizationSlots_.end(), marginalSlots.begin(), marginalSlots.end()); + + // Move the marginalized factors from the filter to the smoother (holding area) + // Note: Be careful to only move nonlinear factors and not any marginals that may also need to be removed + BOOST_FOREACH(size_t slot, removedFactorSlots) { + std::vector::iterator iter = std::find(smootherSummarizationSlots_.begin(), smootherSummarizationSlots_.end(), slot); + if(iter == smootherSummarizationSlots_.end()) { + // This is a real nonlinear factor. Add it to the smoother factor cache. + smootherFactors_.push_back(factors_.at(slot)); + } else { + // This is a marginal factor that was removed and replaced by a new marginal factor. Remove this slot from the separator factor list. + smootherSummarizationSlots_.erase(iter); + } + } + + // Add the linearization point of the moved variables to the smoother cache + BOOST_FOREACH(Key key, keysToMove) { + smootherValues_.insert(key, theta_.at(key)); + } } - // Permute the ordering such that the removed keys are at the end. - // This is a prerequisite for removing them from several structures - std::vector toBack; - BOOST_FOREACH(Key key, keysToMove) { - toBack.push_back(ordering_.at(key)); + // Remove the marginalized variables and factors from the filter + { + // Remove marginalized factors from the factor graph + std::vector slots(removedFactorSlots.begin(), removedFactorSlots.end()); + removeFactors(slots); + + // Remove marginalized keys from values (and separator) + BOOST_FOREACH(Key key, keysToMove) { + theta_.erase(key); + if(separatorValues_.exists(key)) { + separatorValues_.erase(key); + } + } + + // Permute the ordering such that the removed keys are at the end. + // This is a prerequisite for removing them from several structures + std::vector toBack; + BOOST_FOREACH(Key key, keysToMove) { + toBack.push_back(ordering_.at(key)); + } + Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); + ordering_.permuteInPlace(forwardPermutation); + delta_.permuteInPlace(forwardPermutation); + + // Remove marginalized keys from the ordering and delta + for(size_t i = 0; i < keysToMove.size(); ++i) { + ordering_.pop_back(); + delta_.pop_back(); + } } - Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); - ordering_.permuteInPlace(forwardPermutation); - delta_.permuteInPlace(forwardPermutation); - - // Remove marginalized keys from the ordering and delta - for(size_t i = 0; i < keysToMove.size(); ++i) { - ordering_.pop_back(); - delta_.pop_back(); - } - - -// // Calculate marginal factors on the remaining variables (after marginalizing 'keyToMove') -// // Note: It is assumed the ordering already has these keys first -// -// // text -// NonlinearFactorGraph marginalFactors = marginalize(factors_, theta_, ordering_, keysToMove, parameters_.getEliminationFunction()); -// -// // text -// BOOST_FOREACH(const NonlinearFactor::shared_ptr& marginalFactor, marginalFactors) { -// BOOST_FOREACH(Key key, *marginalFactor) { -// if(!separatorValues_.exists(key)) { -// separatorValues_.insert(key, theta_.at(key)); -// } -// } -// } -// std::vector marginalSlots = insertFactors(marginalFactors); -// -// // text -// // Use the variable Index to mark the factors that will be marginalized -// std::set removedFactorSlots; -// BOOST_FOREACH(Key key, keysToMove) { -// const FastList& slots = variableIndex[ordering_.at(key)]; -// removedFactorSlots.insert(slots.begin(), slots.end()); -// } -// -// -// -// -// // Create the linear factor graph -// GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_); -// -// // Calculate the variable index -// VariableIndex variableIndex(linearFactorGraph, ordering_.size()); -// -// // Use the variable Index to mark the factors that will be marginalized -// std::set removedFactorSlots; -// BOOST_FOREACH(Key key, keysToMove) { -// const FastList& slots = variableIndex[ordering_.at(key)]; -// removedFactorSlots.insert(slots.begin(), slots.end()); -// } -// -// // Construct an elimination tree to perform sparse elimination -// std::vector forest( EliminationForest::Create(linearFactorGraph, variableIndex) ); -// -// // This is a tree. Only the top-most nodes/indices need to be eliminated; all of the children will be eliminated automatically -// // Find the subset of nodes/keys that must be eliminated -// std::set indicesToEliminate; -// BOOST_FOREACH(Key key, keysToMove) { -// indicesToEliminate.insert(ordering_.at(key)); -// } -// BOOST_FOREACH(Key key, keysToMove) { -// EliminationForest::removeChildrenIndices(indicesToEliminate, forest.at(ordering_.at(key))); -// } -// -// // Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables -// // Convert the marginal factors into Linear Container Factors -// // Add the marginal factor variables to the separator -// NonlinearFactorGraph marginalFactors; -// BOOST_FOREACH(Index index, indicesToEliminate) { -// GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction()); -// 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)); -// } -// } -// } -// } -// std::vector marginalSlots = insertFactors(marginalFactors); -// -// -// // Cache marginalized variables and factors for later transmission to the smoother -// { -// // Add the new marginal factors to the list of smootherSeparatorFactors. In essence, we have just moved the separator -// smootherSummarizationSlots_.insert(smootherSummarizationSlots_.end(), marginalSlots.begin(), marginalSlots.end()); -// -// // Move the marginalized factors from the filter to the smoother (holding area) -// // Note: Be careful to only move nonlinear factors and not any marginals that may also need to be removed -// BOOST_FOREACH(size_t slot, removedFactorSlots) { -// std::vector::iterator iter = std::find(smootherSummarizationSlots_.begin(), smootherSummarizationSlots_.end(), slot); -// if(iter == smootherSummarizationSlots_.end()) { -// // This is a real nonlinear factor. Add it to the smoother factor cache. -// smootherFactors_.push_back(factors_.at(slot)); -// } else { -// // This is a marginal factor that was removed and replaced by a new marginal factor. Remove this slot from the smoother summarization list. -// smootherSummarizationSlots_.erase(iter); -// } -// } -// -// // Add the linearization point of the moved variables to the smoother cache -// BOOST_FOREACH(Key key, keysToMove) { -// smootherValues_.insert(key, theta_.at(key)); -// } -// } -// -// // Remove the marginalized variables and factors from the filter -// { -// // Remove marginalized factors from the factor graph -// std::vector slots(removedFactorSlots.begin(), removedFactorSlots.end()); -// removeFactors(slots); -// -// // Remove marginalized keys from values (and separator) -// BOOST_FOREACH(Key key, keysToMove) { -// theta_.erase(key); -// if(separatorValues_.exists(key)) { -// separatorValues_.erase(key); -// } -// } -// -// // Permute the ordering such that the removed keys are at the end. -// // This is a prerequisite for removing them from several structures -// std::vector toBack; -// BOOST_FOREACH(Key key, keysToMove) { -// toBack.push_back(ordering_.at(key)); -// } -// Permutation forwardPermutation = Permutation::PushToBack(toBack, ordering_.size()); -// ordering_.permuteInPlace(forwardPermutation); -// delta_.permuteInPlace(forwardPermutation); -// -// // Remove marginalized keys from the ordering and delta -// for(size_t i = 0; i < keysToMove.size(); ++i) { -// ordering_.pop_back(); -// delta_.pop_back(); -// } -// } } /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index 7feee4d82..9a7690786 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -114,7 +114,7 @@ public: * @param newTheta Initialization points for new variables to be added to the filter * You must include here all new variables occurring in newFactors that were not already * in the filter. - * @param keysToMove An optional set of keys to move from the filter to the smoother + * @param keysToMove An optional set of keys to remove from the filter and */ Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const boost::optional >& keysToMove = boost::none); @@ -129,11 +129,7 @@ protected: VectorValues delta_; ///< The current set of linear deltas from the linearization point std::queue availableSlots_; ///< The set of available factor graph slots caused by deleting factors Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization. - std::vector currentSmootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator - - // Storage for information from the Smoother - NonlinearFactorGraph previousSmootherSummarization_; ///< The smoother summarization on the old separator sent by the smoother during the last synchronization - NonlinearFactorGraph smootherShortcut_; ///< A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update) + std::vector smootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization factors // Storage for information to be sent to the smoother NonlinearFactorGraph filterSummarization_; ///< A temporary holding place for calculated filter summarization factors to be sent to the smoother @@ -197,17 +193,15 @@ private: /** Use colamd to update into an efficient ordering */ void reorder(const boost::optional >& keysToMove = boost::none); - /** Marginalize out the set of requested variables from the filter, caching them for the smoother - * This effectively moves the separator. - * - * @param keysToMove The set of keys to move from the filter to the smoother - */ - void moveSeparator(const FastList& keysToMove); - /** Use a modified version of L-M to update the linearization point and delta */ static Result optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering, VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters); + /** Marginalize out the set of requested variables from the filter, caching them for the smoother + * This effectively moves the separator. + */ + void marginalize(const FastList& keysToMove); + /** Marginalize out the set of requested variables from the filter, caching them for the smoother * This effectively moves the separator. */ From 41b0b903768931a63f704be4d8e204e454090544 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 22 May 2013 17:27:42 +0000 Subject: [PATCH 06/47] Added nrNodes() and checkConsistency() to BayesTree --- gtsam.h | 2 ++ gtsam/inference/BayesTree-inl.h | 23 +++++++++++++++++++++-- gtsam/inference/BayesTree.h | 10 ++++++++++ 3 files changed, 33 insertions(+), 2 deletions(-) diff --git a/gtsam.h b/gtsam.h index dbd3d9fad..841c4b01c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -811,6 +811,7 @@ virtual class BayesTree { //Standard Interface //size_t findParentClique(const gtsam::IndexVector& parents) const; size_t size(); + size_t nrNodes() const; void saveGraph(string s) const; CLIQUE* root() const; void clear(); @@ -818,6 +819,7 @@ virtual class BayesTree { void insert(const CLIQUE* subtree); size_t numCachedSeparatorMarginals() const; CLIQUE* clique(size_t j) const; + bool checkConsistency() const; }; template diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 9f672e56f..f3aeacb36 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -799,5 +799,24 @@ namespace gtsam { } } -} -/// namespace gtsam + /* ************************************************************************* */ + template + bool BayesTree::checkConsistency() const { + // Verify all nodes are mapped to initialized cliques + bool result = true; + for (gtsam::Index idx=0; idx Date: Wed, 22 May 2013 17:27:43 +0000 Subject: [PATCH 07/47] Added check to prevent trying to add disconnected factors in iSAM1 --- gtsam/inference/ISAM-inl.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/ISAM-inl.h b/gtsam/inference/ISAM-inl.h index 45031191b..4b5333f33 100644 --- a/gtsam/inference/ISAM-inl.h +++ b/gtsam/inference/ISAM-inl.h @@ -36,8 +36,14 @@ namespace gtsam { const FG& newFactors, Cliques& orphans, typename FG::Eliminate function) { // Remove the contaminated part of the Bayes tree + // Throw exception if disconnected BayesNet bn; - this->removeTop(newFactors.keys(), bn, orphans); + if (!this->empty()) { + this->removeTop(newFactors.keys(), bn, orphans); + if (bn.empty()) + throw std::runtime_error( + "ISAM::update_internal(): no variables in common between existing Bayes tree and incoming factors!"); + } FG factors(bn); // add the factors themselves From 228a26947f180108b355c1a5274b46e473b00183 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 22 May 2013 17:27:44 +0000 Subject: [PATCH 08/47] Removed checkConsistency() from BayesTree --- gtsam.h | 1 - gtsam/inference/BayesTree-inl.h | 18 ------------------ gtsam/inference/BayesTree.h | 7 ------- 3 files changed, 26 deletions(-) diff --git a/gtsam.h b/gtsam.h index 841c4b01c..429cbc7d3 100644 --- a/gtsam.h +++ b/gtsam.h @@ -819,7 +819,6 @@ virtual class BayesTree { void insert(const CLIQUE* subtree); size_t numCachedSeparatorMarginals() const; CLIQUE* clique(size_t j) const; - bool checkConsistency() const; }; template diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index f3aeacb36..d5592ed2a 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -800,23 +800,5 @@ namespace gtsam { } /* ************************************************************************* */ - template - bool BayesTree::checkConsistency() const { - // Verify all nodes are mapped to initialized cliques - bool result = true; - for (gtsam::Index idx=0; idx Date: Thu, 23 May 2013 18:12:00 +0000 Subject: [PATCH 09/47] Caught corner case in summarization causing ccolamd to segfault --- gtsam/nonlinear/summarization.cpp | 19 +++++++------------ gtsam/nonlinear/summarization.h | 3 +++ tests/testSummarization.cpp | 27 +++++++++++++++++++++++---- 3 files changed, 33 insertions(+), 16 deletions(-) diff --git a/gtsam/nonlinear/summarization.cpp b/gtsam/nonlinear/summarization.cpp index 5722f9c2c..acd222d43 100644 --- a/gtsam/nonlinear/summarization.cpp +++ b/gtsam/nonlinear/summarization.cpp @@ -18,18 +18,13 @@ std::pair summarize(const NonlinearFactorGraph& graph, const Values& values, const KeySet& saved_keys, SummarizationMode mode) { const size_t nrEliminatedKeys = values.size() - saved_keys.size(); - // // compute a new ordering with non-saved keys first - // Ordering ordering; - // KeySet eliminatedKeys; - // BOOST_FOREACH(const Key& key, values.keys()) { - // if (!saved_keys.count(key)) { - // eliminatedKeys.insert(key); - // ordering += key; - // } - // } - // - // BOOST_FOREACH(const Key& key, saved_keys) - // ordering += key; + + // If we aren't eliminating anything, linearize and return + if (!nrEliminatedKeys || saved_keys.empty()) { + Ordering ordering = *values.orderingArbitrary(); + GaussianFactorGraph linear_graph = *graph.linearize(values, ordering); + return make_pair(linear_graph, ordering); + } // Compute a constrained ordering with variables grouped to end std::map ordering_constraints; diff --git a/gtsam/nonlinear/summarization.h b/gtsam/nonlinear/summarization.h index faa2003ef..c25d5d934 100644 --- a/gtsam/nonlinear/summarization.h +++ b/gtsam/nonlinear/summarization.h @@ -29,6 +29,9 @@ typedef enum { * Summarization function to remove a subset of variables from a system with the * sequential solver. This does not require that the system be fully constrained. * + * Requirement: set of keys in the graph should match the set of keys in the + * values structure. + * * @param graph A full nonlinear graph * @param values The chosen linearization point * @param saved_keys is the set of keys for variables that should remain diff --git a/tests/testSummarization.cpp b/tests/testSummarization.cpp index 2f3cfdf03..bf7342202 100644 --- a/tests/testSummarization.cpp +++ b/tests/testSummarization.cpp @@ -34,17 +34,16 @@ typedef gtsam::PriorFactor PosePrior; typedef gtsam::BetweenFactor PoseBetween; typedef gtsam::BearingRangeFactor PosePointBearingRange; +gtsam::noiseModel::Base::shared_ptr model2 = noiseModel::Unit::Create(2); +gtsam::noiseModel::Base::shared_ptr model3 = noiseModel::Unit::Create(3); + /* ************************************************************************* */ TEST( testSummarization, example_from_ddf1 ) { - Key xA0 = LabeledSymbol('x', 'A', 0), xA1 = LabeledSymbol('x', 'A', 1), xA2 = LabeledSymbol('x', 'A', 2); Key lA3 = LabeledSymbol('l', 'A', 3), lA5 = LabeledSymbol('l', 'A', 5); - gtsam::noiseModel::Base::shared_ptr model2 = noiseModel::Unit::Create(2); - gtsam::noiseModel::Base::shared_ptr model3 = noiseModel::Unit::Create(3); - SharedDiagonal diagmodel2 = noiseModel::Unit::Create(2); SharedDiagonal diagmodel4 = noiseModel::Unit::Create(4); @@ -218,6 +217,26 @@ TEST( testSummarization, example_from_ddf1 ) { } } +/* ************************************************************************* */ +TEST( testSummarization, no_summarize_case ) { + // Checks a corner case in which no variables are being eliminated + gtsam::Key key = 7; + gtsam::KeySet saved_keys; saved_keys.insert(key); + NonlinearFactorGraph graph; + graph.add(PosePrior(key, Pose2(1.0, 2.0, 0.3), model3)); + graph.add(PosePrior(key, Pose2(2.0, 3.0, 0.4), model3)); + Values values; + values.insert(key, Pose2(0.0, 0.0, 0.1)); + + SummarizationMode mode = SEQUENTIAL_CHOLESKY; + GaussianFactorGraph actLinGraph; Ordering actOrdering; + boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode); + Ordering expOrdering; expOrdering += key; + GaussianFactorGraph expLinGraph = *graph.linearize(values, expOrdering); + EXPECT(assert_equal(expOrdering, actOrdering)); + EXPECT(assert_equal(expLinGraph, actLinGraph)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 045a3d281f3c704f003cc3034d40ed353dbbe22f Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 4 Jun 2013 13:37:12 +0000 Subject: [PATCH 10/47] Only use Boost dynamic auto-linking flag on Windows --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e4b6efb0e..bdbd25abf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -100,7 +100,7 @@ endif() # Find boost # If using Boost shared libs, set up auto linking for shared libs -if(NOT Boost_USE_STATIC_LIBS) +if(MSVC AND NOT Boost_USE_STATIC_LIBS) add_definitions(-DBOOST_ALL_DYN_LINK) endif() From ac50d39a97c58348ef51ffe182816ec28e3b3fec Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 4 Jun 2013 13:37:13 +0000 Subject: [PATCH 11/47] Swapped group name and test name in CppUnitLite to match the convention we use in unit tests --- CppUnitLite/Test.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 52b79f65c..9f97757b6 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -60,7 +60,7 @@ protected: /** * Normal test will wrap execution in a try/catch block to catch exceptions more effectively */ -#define TEST(testName, testGroup)\ +#define TEST(testGroup, testName)\ class testGroup##testName##Test : public Test \ { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \ virtual ~testGroup##testName##Test () {};\ @@ -72,7 +72,7 @@ protected: * For debugging only: use TEST_UNSAFE to allow debuggers to have access to exceptions, as this * will not wrap execution with a try/catch block */ -#define TEST_UNSAFE(testName, testGroup)\ +#define TEST_UNSAFE(testGroup, testName)\ class testGroup##testName##Test : public Test \ { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \ virtual ~testGroup##testName##Test () {};\ From b3282577fbb1aa14637ecb36974af812a0c52830 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 4 Jun 2013 14:07:44 +0000 Subject: [PATCH 13/47] Revert "Swapped group name and test name in CppUnitLite to match the convention we use in unit tests" This reverts commit c186908e755d034cc821376fd78c7118c9ccf48a. --- CppUnitLite/Test.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 9f97757b6..52b79f65c 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -60,7 +60,7 @@ protected: /** * Normal test will wrap execution in a try/catch block to catch exceptions more effectively */ -#define TEST(testGroup, testName)\ +#define TEST(testName, testGroup)\ class testGroup##testName##Test : public Test \ { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \ virtual ~testGroup##testName##Test () {};\ @@ -72,7 +72,7 @@ protected: * For debugging only: use TEST_UNSAFE to allow debuggers to have access to exceptions, as this * will not wrap execution with a try/catch block */ -#define TEST_UNSAFE(testGroup, testName)\ +#define TEST_UNSAFE(testName, testGroup)\ class testGroup##testName##Test : public Test \ { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \ virtual ~testGroup##testName##Test () {};\ From b2d4469cb3ae993bad52f83d7f5f533110d032d2 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 4 Jun 2013 17:34:02 +0000 Subject: [PATCH 14/47] Added missing export tag --- gtsam_unstable/slam/AHRS.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index 284fe2fc6..01c61704b 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -14,7 +14,7 @@ namespace gtsam { -Matrix cov(const Matrix& m); +GTSAM_UNSTABLE_EXPORT Matrix cov(const Matrix& m); class GTSAM_UNSTABLE_EXPORT AHRS { From 84903d05c2846014fda650921e5f948ea60774cb Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 4 Jun 2013 17:34:03 +0000 Subject: [PATCH 15/47] Fixed iSAM2 bug where assignment operator and copy constructor may cause null pointer exception when trying to clone cached linear factors, which become null by calling marginalizeLeaves with linear factor caching enabled. --- gtsam/nonlinear/ISAM2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 9b063f0ce..a209c3d44 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -125,7 +125,7 @@ ISAM2& ISAM2::operator=(const ISAM2& rhs) { linearFactors_ = GaussianFactorGraph(); linearFactors_.reserve(rhs.linearFactors_.size()); BOOST_FOREACH(const GaussianFactor::shared_ptr& linearFactor, rhs.linearFactors_) { - linearFactors_.push_back(linearFactor->clone()); } + linearFactors_.push_back(linearFactor ? linearFactor->clone() : GaussianFactor::shared_ptr()); } ordering_ = rhs.ordering_; params_ = rhs.params_; From 7fcdc467c17b081498ffc778890becd651eca6a3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 5 Jun 2013 16:10:16 +0000 Subject: [PATCH 16/47] Added stream operator << and renamed dist to distance --- gtsam/geometry/Point2.cpp | 6 ++++++ gtsam/geometry/Point2.h | 8 ++++++++ 2 files changed, 14 insertions(+) diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 444f5d759..e188b2930 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -40,4 +40,10 @@ double Point2::norm() const { return sqrt(x_*x_ + y_*y_); } +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Point2& p) { + os << '(' << p.x() << ", " << p.y() << ')'; + return os; +} + } // namespace gtsam diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 850f84b01..74ea16638 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -151,6 +151,11 @@ public: Point2 unit() const { return *this/norm(); } /** distance between two points */ + inline double distance(const Point2& p2) const { + return (p2 - *this).norm(); + } + + /** @deprecated The following function has been deprecated, use distance above */ inline double dist(const Point2& p2) const { return (p2 - *this).norm(); } @@ -184,6 +189,9 @@ public: inline void operator *= (double s) {x_*=s;y_*=s;} /// @} + /// Streaming + friend std::ostream &operator<<(std::ostream &os, const Point2& p); + private: /// @name Advanced Interface From ebcc638ef5e07d9630c6b5112c6de6e27212c985 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 5 Jun 2013 16:11:36 +0000 Subject: [PATCH 17/47] renamed dist to distance (dist still works but now deprecated) --- gtsam/geometry/Cal3DS2.cpp | 2 +- gtsam/geometry/Point3.h | 9 +++++++-- gtsam/geometry/tests/testPoint2.cpp | 17 +++++++++++++---- gtsam_unstable/dynamics/PoseRTV.cpp | 2 +- gtsam_unstable/geometry/SimPolygon2D.cpp | 6 +++--- gtsam_unstable/geometry/SimWall2D.cpp | 4 ++-- gtsam_unstable/geometry/SimWall2D.h | 2 +- 7 files changed, 28 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index 6816346e8..1d5ad695a 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -83,7 +83,7 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const { const int maxIterations = 10; int iteration; for ( iteration = 0 ; iteration < maxIterations ; ++iteration ) { - if ( uncalibrate(pn).dist(pi) <= tol ) break; + if ( uncalibrate(pn).distance(pi) <= tol ) break; const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y ; const double rr = xx + yy ; const double g = (1+k1_*rr+k2_*rr*rr) ; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 7e0c59ba9..4713dacab 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -153,8 +153,13 @@ namespace gtsam { Point3 operator / (double s) const; /** distance between two points */ - double dist(const Point3& p2) const { - return std::sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0)); + inline double distance(const Point3& p2) const { + return (p2 - *this).norm(); + } + + /** @deprecated The following function has been deprecated, use distance above */ + inline double dist(const Point3& p2) const { + return (p2 - *this).norm(); } /** Distance of the point from the origin */ diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 446255803..7c73b28a2 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -75,19 +75,28 @@ TEST( Point2, norm) Point2 p0(cos(5.0), sin(5.0)); DOUBLES_EQUAL(1,p0.norm(),1e-6); Point2 p1(4, 5), p2(1, 1); - DOUBLES_EQUAL( 5,p1.dist(p2),1e-6); + DOUBLES_EQUAL( 5,p1.distance(p2),1e-6); DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6); } /* ************************************************************************* */ TEST( Point2, unit) { - Point2 p0(10.0, 0.0), p1(0.0,-10.0), p2(10.0, 10.0); - EXPECT(assert_equal(Point2(1.0, 0.0), p0.unit(), 1e-6)); - EXPECT(assert_equal(Point2(0.0,-1.0), p1.unit(), 1e-6)); + Point2 p0(10, 0), p1(0,-10), p2(10, 10); + EXPECT(assert_equal(Point2(1, 0), p0.unit(), 1e-6)); + EXPECT(assert_equal(Point2(0,-1), p1.unit(), 1e-6)); EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6)); } +/* ************************************************************************* */ +TEST( Point2, stream) +{ + Point2 p(1,2); + std::ostringstream os; + os << p; + EXPECT(os.str() == "(1, 2)"); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 5a442560e..dd2671530 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -232,7 +232,7 @@ double PoseRTV::range(const PoseRTV& other, boost::optional H1, boost::optional H2) const { if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5); if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5); - return t().dist(other.t()); + return t().distance(other.t()); } /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/SimPolygon2D.cpp b/gtsam_unstable/geometry/SimPolygon2D.cpp index bc22cb024..85b592b98 100644 --- a/gtsam_unstable/geometry/SimPolygon2D.cpp +++ b/gtsam_unstable/geometry/SimPolygon2D.cpp @@ -152,7 +152,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( Pose2 xC = xB.retract(delta(3, 0, dBC)); // use triangle equality to verify non-degenerate triangle - double dAC = xA.t().dist(xC.t()); + double dAC = xA.t().distance(xC.t()); // form a triangle and test if it meets requirements SimPolygon2D test_tri = SimPolygon2D::createTriangle(xA.t(), xB.t(), xC.t()); @@ -165,7 +165,7 @@ SimPolygon2D SimPolygon2D::randomTriangle( insideBox(side_len, test_tri.landmark(0)) && insideBox(side_len, test_tri.landmark(1)) && insideBox(side_len, test_tri.landmark(2)) && - test_tri.landmark(1).dist(test_tri.landmark(2)) > min_side_len && + test_tri.landmark(1).distance(test_tri.landmark(2)) > min_side_len && !nearExisting(lms, test_tri.landmark(0), min_vertex_dist) && !nearExisting(lms, test_tri.landmark(1), min_vertex_dist) && !nearExisting(lms, test_tri.landmark(2), min_vertex_dist) && @@ -321,7 +321,7 @@ bool SimPolygon2D::insideBox(double s, const Point2& p) { bool SimPolygon2D::nearExisting(const std::vector& S, const Point2& p, double threshold) { BOOST_FOREACH(const Point2& Sp, S) - if (Sp.dist(p) < threshold) + if (Sp.distance(p) < threshold) return true; return false; } diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index b03f52890..78347a077 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -139,7 +139,7 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, Point2 cur_intersection; if (wall.intersects(traj,cur_intersection)) { collision = true; - if (cur_pose.t().dist(cur_intersection) < cur_pose.t().dist(intersection)) { + if (cur_pose.t().distance(cur_intersection) < cur_pose.t().distance(intersection)) { intersection = cur_intersection; closest_wall = wall; } @@ -155,7 +155,7 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, norm = norm / norm.norm(); // Simple check to make sure norm is on side closest robot - if (cur_pose.t().dist(intersection + norm) > cur_pose.t().dist(intersection - norm)) + if (cur_pose.t().distance(intersection + norm) > cur_pose.t().distance(intersection - norm)) norm = norm.inverse(); // using the reflection diff --git a/gtsam_unstable/geometry/SimWall2D.h b/gtsam_unstable/geometry/SimWall2D.h index d3e86296a..cf2142af8 100644 --- a/gtsam_unstable/geometry/SimWall2D.h +++ b/gtsam_unstable/geometry/SimWall2D.h @@ -43,7 +43,7 @@ namespace gtsam { SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); } /** geometry */ - double length() const { return a_.dist(b_); } + double length() const { return a_.distance(b_); } Point2 midpoint() const; /** From 0789954ec5e410efa0a3f097b584d4ca823aafad Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 5 Jun 2013 18:43:54 +0000 Subject: [PATCH 18/47] Added stream operator << --- gtsam/geometry/Point3.cpp | 21 ++++++++++++++++++--- gtsam/geometry/Point3.h | 10 +++++++--- gtsam/geometry/tests/testPoint3.cpp | 9 +++++++++ 3 files changed, 34 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 95c7582c6..c8ee78565 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -14,10 +14,11 @@ * @brief 3D Point */ -#include #include #include +using namespace std; + namespace gtsam { /** Explicit instantiation of base class to export members */ @@ -30,8 +31,8 @@ bool Point3::equals(const Point3 & q, double tol) const { /* ************************************************************************* */ -void Point3::print(const std::string& s) const { - std::cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << std::endl; +void Point3::print(const string& s) const { + cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << endl; } /* ************************************************************************* */ @@ -49,14 +50,17 @@ Point3 Point3::operator+(const Point3& q) const { Point3 Point3::operator- (const Point3& q) const { return Point3( x_ - q.x_, y_ - q.y_, z_ - q.z_ ); } + /* ************************************************************************* */ Point3 Point3::operator*(double s) const { return Point3(x_ * s, y_ * s, z_ * s); } + /* ************************************************************************* */ Point3 Point3::operator/(double s) const { return Point3(x_ / s, y_ / s, z_ / s); } + /* ************************************************************************* */ Point3 Point3::add(const Point3 &q, boost::optional H1, boost::optional H2) const { @@ -64,6 +68,7 @@ Point3 Point3::add(const Point3 &q, if (H2) *H2 = eye(3,3); return *this + q; } + /* ************************************************************************* */ Point3 Point3::sub(const Point3 &q, boost::optional H1, boost::optional H2) const { @@ -71,20 +76,30 @@ Point3 Point3::sub(const Point3 &q, if (H2) *H2 = -eye(3,3); return *this - q; } + /* ************************************************************************* */ Point3 Point3::cross(const Point3 &q) const { return Point3( y_*q.z_ - z_*q.y_, z_*q.x_ - x_*q.z_, x_*q.y_ - y_*q.x_ ); } + /* ************************************************************************* */ double Point3::dot(const Point3 &q) const { return ( x_*q.x_ + y_*q.y_ + z_*q.z_ ); } + /* ************************************************************************* */ double Point3::norm() const { return sqrt( x_*x_ + y_*y_ + z_*z_ ); } + +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Point3& p) { + os << '(' << p.x() << ", " << p.y() << ", " << p.z() << ')'; + return os; +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 4713dacab..cf30bd1ff 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -21,13 +21,14 @@ #pragma once -#include -#include - #include #include #include +#include + +#include + namespace gtsam { /** @@ -202,6 +203,9 @@ namespace gtsam { /// @} + /// Output stream operator + friend std::ostream &operator<<(std::ostream &os, const Point3& p); + private: /// @name Advanced Interface diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 07152df36..b50e1b9d9 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -71,6 +71,15 @@ TEST( Point3, dot) CHECK(ones.dot(Point3(1,1,0)) == 2); } +/* ************************************************************************* */ +TEST( Point3, stream) +{ + Point3 p(1,2, -3); + std::ostringstream os; + os << p; + EXPECT(os.str() == "(1, 2, -3)"); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From f4ad135040248a432bffc3431eb4e75c3f03fd5e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 5 Jun 2013 23:39:42 +0000 Subject: [PATCH 19/47] testVelocityConstraint3.run target --- .cproject | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.cproject b/.cproject index e5b4e113a..ff425c83f 100644 --- a/.cproject +++ b/.cproject @@ -453,6 +453,14 @@ true true + + make + -j5 + testVelocityConstraint3.run + true + true + true + make -j5 From 64334e25842ca47dcda2372da2201f8a1df24496 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 5 Jun 2013 23:40:05 +0000 Subject: [PATCH 20/47] smarter compose --- gtsam/geometry/Rot2.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 591e5c8a0..45012770f 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -116,11 +116,11 @@ namespace gtsam { } /** Compose - make a new rotation by adding angles */ - inline Rot2 compose(const Rot2& R1, boost::optional H1 = + inline Rot2 compose(const Rot2& R, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { if (H1) *H1 = eye(1); if (H2) *H2 = eye(1); - return *this * R1; + return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); } /** Compose - make a new rotation by adding angles */ @@ -129,11 +129,11 @@ namespace gtsam { } /** Between using the default implementation */ - inline Rot2 between(const Rot2& p2, boost::optional H1 = + inline Rot2 between(const Rot2& R, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { if (H1) *H1 = -eye(1); if (H2) *H2 = eye(1); - return between_default(*this, p2); + return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); } /// @} From 640fcd94b11bc836fac15e54d7a80f1e39fd7530 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 5 Jun 2013 23:41:46 +0000 Subject: [PATCH 21/47] Added stream operator << --- .cproject | 358 ++++++++++++++--------------- gtsam/geometry/Rot3.h | 3 + gtsam/geometry/Rot3M.cpp | 9 + gtsam/geometry/Rot3Q.cpp | 9 + gtsam/geometry/tests/testRot3M.cpp | 9 + gtsam/geometry/tests/testRot3Q.cpp | 9 + 6 files changed, 217 insertions(+), 180 deletions(-) diff --git a/.cproject b/.cproject index ff425c83f..872909c06 100644 --- a/.cproject +++ b/.cproject @@ -309,6 +309,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -335,7 +343,6 @@ make - tests/testBayesTree.run true false @@ -343,7 +350,6 @@ make - testBinaryBayesNet.run true false @@ -391,7 +397,6 @@ make - testSymbolicBayesNet.run true false @@ -399,7 +404,6 @@ make - tests/testSymbolicFactor.run true false @@ -407,7 +411,6 @@ make - testSymbolicFactorGraph.run true false @@ -423,20 +426,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -533,22 +527,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -565,6 +543,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -589,26 +583,34 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run true true true @@ -693,34 +695,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run + -j2 + clean true true true @@ -1079,7 +1073,6 @@ make - testGraph.run true false @@ -1087,7 +1080,6 @@ make - testJunctionTree.run true false @@ -1095,7 +1087,6 @@ make - testSymbolicBayesNetB.run true false @@ -1263,7 +1254,6 @@ make - testErrors.run true false @@ -1309,6 +1299,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1389,14 +1387,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1621,6 +1611,14 @@ true true + + make + -j5 + testRot3Q.run + true + true + true + make -j2 @@ -1703,6 +1701,7 @@ make + testSimulated2DOriented.run true false @@ -1742,6 +1741,7 @@ make + testSimulated2D.run true false @@ -1749,6 +1749,7 @@ make + testSimulated3D.run true false @@ -1948,6 +1949,7 @@ make + tests/testGaussianISAM2 true false @@ -1969,6 +1971,102 @@ true true + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + make -j3 @@ -2170,7 +2268,6 @@ cpack - -G DEB true false @@ -2178,7 +2275,6 @@ cpack - -G RPM true false @@ -2186,7 +2282,6 @@ cpack - -G TGZ true false @@ -2194,7 +2289,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2336,98 +2430,34 @@ true true - + make - -j2 - testRot3.run + -j5 + testSpirit.run true true true - + make - -j2 - testRot2.run + -j5 + testWrap.run true true true - + make - -j2 - testPose3.run + -j5 + check.wrap true true true - + make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run + -j5 + wrap true true true @@ -2471,38 +2501,6 @@ false true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap - true - true - true - diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index e9c7c6b64..6adcd8a1b 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -355,6 +355,9 @@ namespace gtsam { */ Vector quaternion() const; + /// Output stream operator + friend std::ostream &operator<<(std::ostream &os, const Rot3& p); + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index b63499d6d..cb6660dae 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -380,6 +380,15 @@ pair RQ(const Matrix3& A) { return make_pair(R, xyz); } +/* ************************************************************************* */ +ostream &operator<<(ostream &os, const Rot3& R) { + os << "\n"; + os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n"; + os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n"; + os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n"; + return os; +} + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index afab3900c..72d50484b 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -237,6 +237,15 @@ namespace gtsam { return make_pair(R, xyz); } + /* ************************************************************************* */ + ostream &operator<<(ostream &os, const Rot3& R) { + os << "\n"; + os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n"; + os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n"; + os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n"; + return os; + } + } // namespace gtsam #endif diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index f59455e2f..b50ad1253 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -562,6 +562,15 @@ TEST( Rot3, Cayley ) { EXPECT(assert_equal(A, Cayley(Q))); } +/* ************************************************************************* */ +TEST( Rot3, stream) +{ + Rot3 R; + std::ostringstream os; + os << R; + EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n"); +} + #endif /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index e8733c24d..d44b8f34c 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -475,6 +475,15 @@ TEST(Rot3, quaternion) { EXPECT(assert_equal(expected2, actual2)); } +/* ************************************************************************* */ +TEST( Rot3, stream) +{ + Rot3 R; + std::ostringstream os; + os << R; + EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n"); +} + #endif /* ************************************************************************* */ From 1251ba9abb7e27ea49e8ba56d3a410eb16c07507 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 6 Jun 2013 01:21:34 +0000 Subject: [PATCH 22/47] ostream << operator --- gtsam/geometry/Pose3.cpp | 7 +++++++ gtsam/geometry/Pose3.h | 3 +++ gtsam/geometry/tests/testPose3.cpp | 9 +++++++++ 3 files changed, 19 insertions(+) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index adaa2ce69..faec92a6b 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -348,4 +348,11 @@ namespace gtsam { Point3 t = Point3(cq) - R * Point3(cp); return Pose3(R, t); } + + /* ************************************************************************* */ + std::ostream &operator<<(std::ostream &os, const Pose3& pose) { + os << pose.rotation() << "\n" << pose.translation() << endl; + return os; + } + } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 9d7444233..234e2cad6 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -290,6 +290,9 @@ namespace gtsam { */ static std::pair rotationInterval() { return std::make_pair(0, 2); } + /// Output stream operator + friend std::ostream &operator<<(std::ostream &os, const Pose3& p); + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index ed8f850d2..5f4a68d3b 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -741,6 +741,15 @@ TEST( Pose3, adjointTranspose) { EXPECT(assert_equal(numericalH,actualH,1e-5)); } +/* ************************************************************************* */ +TEST( Pose3, stream) +{ + Pose3 T; + std::ostringstream os; + os << T; + EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n\n(0, 0, 0)\n"); +} + /* ************************************************************************* */ int main(){ TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From 709d18a83dc4e1dbe1f11de54a737e9cf5cd494f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 6 Jun 2013 02:09:52 +0000 Subject: [PATCH 23/47] A more modern (and presumably faster) Eigen syntax --- gtsam/base/numericalDerivative.h | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 429920e1c..053a89da8 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -32,8 +32,6 @@ #include #include - - namespace gtsam { /* @@ -106,8 +104,8 @@ namespace gtsam { for (size_t j=0;j Date: Thu, 6 Jun 2013 02:23:20 +0000 Subject: [PATCH 24/47] added documentation for noisemodel --- matlab/+gtsam/Contents.m | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index 7d469d704..de7f97351 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -29,6 +29,11 @@ % GaussianBayesTreeClique - class GaussianBayesTreeClique, see Doxygen page for details % GaussianBayesTree - class GaussianBayesTree, see Doxygen page for details % GaussianISAM - class GaussianISAM, see Doxygen page for details +% noiseModel.Gaussian - class noiseModel.Gaussian, see Doxygen page for details +% noiseModel.Diagonal - class noiseModel.Diagonal, see Doxygen page for details +% noiseModel.Constrained - class noiseModel.Constrained, see Doxygen page for details +% noiseModel.Isotropic - class noiseModel.Isotropic, see Doxygen page for details +% noiseModel.Unit - class noiseModel.Unit, see Doxygen page for details % %% Linear Inference % GaussianSequentialSolver - class GaussianSequentialSolver, see Doxygen page for details From 43a0367a66d2a1ff6ef9afea9492e27651feaeed Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 6 Jun 2013 05:01:16 +0000 Subject: [PATCH 25/47] Commented out MATLAB wrap Rot3::retractCayley because it does not exist in quaternion mode --- gtsam.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index 429cbc7d3..e211068ad 100644 --- a/gtsam.h +++ b/gtsam.h @@ -397,7 +397,7 @@ virtual class Rot3 : gtsam::Value { // Manifold static size_t Dim(); size_t dim() const; - gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options + //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options gtsam::Rot3 retract(Vector v) const; Vector localCoordinates(const gtsam::Rot3& p) const; From ee21ef61a6cfbaf0c53dd059c73cba053b4748c4 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 6 Jun 2013 18:07:55 +0000 Subject: [PATCH 26/47] Added exists() to FactorGraph to allow for checking whether a factor exists at a given index. Necessary for matlab interface. --- gtsam.h | 5 ++++- gtsam/inference/FactorGraph.h | 3 +++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/gtsam.h b/gtsam.h index e211068ad..8ff9056f9 100644 --- a/gtsam.h +++ b/gtsam.h @@ -895,6 +895,7 @@ class SymbolicFactorGraph { void print(string s) const; bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; size_t size() const; + bool exists(size_t i) const; // Standard interface // FIXME: Must wrap FastSet for this to work @@ -1282,6 +1283,7 @@ class GaussianFactorGraph { bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; size_t size() const; gtsam::GaussianFactor* at(size_t idx) const; + bool exists(size_t idx) const; // Inference pair eliminateFrontals(size_t nFrontals) const; @@ -1508,7 +1510,8 @@ class NonlinearFactorGraph { bool empty() const; void remove(size_t i); size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t i) const; + gtsam::NonlinearFactor* at(size_t idx) const; + bool exists(size_t idx) const; void push_back(const gtsam::NonlinearFactorGraph& factors); // NonlinearFactorGraph diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 70954152f..0d3b10e31 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -165,6 +165,9 @@ class VariableIndex; const sharedFactor operator[](size_t i) const { return at(i); } sharedFactor& operator[](size_t i) { return at(i); } + /** Checks whether a valid factor exists at the given index */ + inline bool exists(size_t i) const { return i Date: Fri, 7 Jun 2013 14:03:27 +0000 Subject: [PATCH 27/47] Used << --- gtsam/geometry/Point2.cpp | 2 +- gtsam/geometry/Point3.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index e188b2930..27a962e70 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -27,7 +27,7 @@ INSTANTIATE_LIE(Point2); /* ************************************************************************* */ void Point2::print(const string& s) const { - cout << s << "(" << x_ << ", " << y_ << ")" << endl; + cout << s << *this << endl; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index c8ee78565..df31f0803 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -32,7 +32,7 @@ bool Point3::equals(const Point3 & q, double tol) const { /* ************************************************************************* */ void Point3::print(const string& s) const { - cout << s << "(" << x_ << ", " << y_ << ", " << z_ << ")" << endl; + cout << s << *this << endl; } /* ************************************************************************* */ From b3748cf7c6724194413313ca40e49b4af8bbd352 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 10 Jun 2013 20:49:47 +0000 Subject: [PATCH 28/47] Added access functions to measurements and noisemodels for a variety of common nonlinear factors --- gtsam.h | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/gtsam.h b/gtsam.h index 8ff9056f9..20ea0bf8a 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1958,6 +1958,8 @@ class NonlinearISAM { template virtual class PriorFactor : gtsam::NonlinearFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + T prior() const; + gtsam::noiseModel::Base* get_noiseModel() const; }; @@ -1965,6 +1967,8 @@ virtual class PriorFactor : gtsam::NonlinearFactor { template virtual class BetweenFactor : gtsam::NonlinearFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); + T measured() const; + gtsam::noiseModel::Base* get_noiseModel() const; }; @@ -1982,6 +1986,7 @@ virtual class NonlinearEquality : gtsam::NonlinearFactor { template virtual class RangeFactor : gtsam::NonlinearFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); + gtsam::noiseModel::Base* get_noiseModel() const; }; typedef gtsam::RangeFactor RangeFactorPosePoint2; @@ -1998,6 +2003,7 @@ typedef gtsam::RangeFactor RangeFactor template virtual class BearingFactor : gtsam::NonlinearFactor { BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel); + gtsam::noiseModel::Base* get_noiseModel() const; }; typedef gtsam::BearingFactor BearingFactor2D; @@ -2007,6 +2013,7 @@ typedef gtsam::BearingFactor BearingFa template virtual class BearingRangeFactor : gtsam::NonlinearFactor { BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel); + gtsam::noiseModel::Base* get_noiseModel() const; }; typedef gtsam::BearingRangeFactor BearingRangeFactor2D; @@ -2030,6 +2037,7 @@ virtual class GenericProjectionFactor : gtsam::NonlinearFactor { CALIBRATION* calibration() const; bool verboseCheirality() const; bool throwCheirality() const; + gtsam::noiseModel::Base* get_noiseModel() const; }; typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; @@ -2058,6 +2066,7 @@ virtual class GenericStereoFactor : gtsam::NonlinearFactor { size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); gtsam::StereoPoint2 measured() const; gtsam::Cal3_S2Stereo* calibration() const; + gtsam::noiseModel::Base* get_noiseModel() const; }; typedef gtsam::GenericStereoFactor GenericStereoFactor3D; @@ -2065,6 +2074,7 @@ typedef gtsam::GenericStereoFactor GenericStereoFac template virtual class PoseTranslationPrior : gtsam::NonlinearFactor { PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); + gtsam::noiseModel::Base* get_noiseModel() const; }; typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; @@ -2074,6 +2084,7 @@ typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; template virtual class PoseRotationPrior : gtsam::NonlinearFactor { PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); + gtsam::noiseModel::Base* get_noiseModel() const; }; typedef gtsam::PoseRotationPrior PoseRotationPrior2D; From 7b79cfc38c79dbddc6a48e2791568d47e43b264a Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 11 Jun 2013 14:35:22 +0000 Subject: [PATCH 29/47] Removed all non-test/timing cpp files from tests folder, small example now is header-only --- tests/CMakeLists.txt | 10 - tests/simulated2D.cpp | 49 ---- tests/simulated2D.h | 17 +- tests/simulated2DOriented.cpp | 39 --- tests/simulated2DOriented.h | 9 +- tests/simulated3D.cpp | 43 --- tests/simulated3D.h | 17 +- tests/smallExample.cpp | 502 -------------------------------- tests/smallExample.h | 534 ++++++++++++++++++++++++++++++++++ 9 files changed, 569 insertions(+), 651 deletions(-) delete mode 100644 tests/simulated2D.cpp delete mode 100644 tests/simulated2DOriented.cpp delete mode 100644 tests/simulated3D.cpp delete mode 100644 tests/smallExample.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 0ebada21e..021a7dd97 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,15 +1,5 @@ -# Build a library of example domains, just for tests -file(GLOB test_lib_srcs "*.cpp") -file(GLOB test_srcs "test*.cpp") -file(GLOB time_srcs "time*.cpp") -list(REMOVE_ITEM test_lib_srcs ${test_srcs}) -list(REMOVE_ITEM test_lib_srcs ${time_srcs}) -add_library(test_lib STATIC ${test_lib_srcs}) -target_link_libraries(test_lib ${gtsam-default}) - # Assemble local libraries set(tests_local_libs - test_lib slam nonlinear linear diff --git a/tests/simulated2D.cpp b/tests/simulated2D.cpp deleted file mode 100644 index 199a6756b..000000000 --- a/tests/simulated2D.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file simulated2D.cpp - * @brief measurement functions and derivatives for simulated 2D robot - * @author Frank Dellaert - */ - -#include - -namespace simulated2D { - - static Matrix I = gtsam::eye(2); - - /* ************************************************************************* */ - Point2 prior(const Point2& x, boost::optional H) { - if (H) *H = I; - return x; - } - - /* ************************************************************************* */ - Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1, - boost::optional H2) { - if (H1) *H1 = -I; - if (H2) *H2 = I; - return x2 - x1; - } - - /* ************************************************************************* */ - Point2 mea(const Point2& x, const Point2& l, boost::optional H1, - boost::optional H2) { - if (H1) *H1 = -I; - if (H2) *H2 = I; - return l - x; - } - -/* ************************************************************************* */ - -} // namespace simulated2D - diff --git a/tests/simulated2D.h b/tests/simulated2D.h index 3075d4a4a..f7a9a9284 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -90,7 +90,10 @@ namespace simulated2D { } /// Prior on a single pose, optionally returns derivative - Point2 prior(const Point2& x, boost::optional H = boost::none); + Point2 prior(const Point2& x, boost::optional H = boost::none) { + if (H) *H = gtsam::eye(2); + return x; + } /// odometry between two poses inline Point2 odo(const Point2& x1, const Point2& x2) { @@ -99,7 +102,11 @@ namespace simulated2D { /// odometry between two poses, optionally returns derivative Point2 odo(const Point2& x1, const Point2& x2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none); + boost::none, boost::optional H2 = boost::none) { + if (H1) *H1 = -gtsam::eye(2); + if (H2) *H2 = gtsam::eye(2); + return x2 - x1; + } /// measurement between landmark and pose inline Point2 mea(const Point2& x, const Point2& l) { @@ -108,7 +115,11 @@ namespace simulated2D { /// measurement between landmark and pose, optionally returns derivative Point2 mea(const Point2& x, const Point2& l, boost::optional H1 = - boost::none, boost::optional H2 = boost::none); + boost::none, boost::optional H2 = boost::none) { + if (H1) *H1 = -gtsam::eye(2); + if (H2) *H2 = gtsam::eye(2); + return l - x; + } /** * Unary factor encoding a soft prior on a vector diff --git a/tests/simulated2DOriented.cpp b/tests/simulated2DOriented.cpp deleted file mode 100644 index c7d1d8469..000000000 --- a/tests/simulated2DOriented.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file simulated2DOriented - * @brief measurement functions and derivatives for simulated 2D robot - * @author Frank Dellaert - */ - -#include - -namespace simulated2DOriented { - - static Matrix I = gtsam::eye(3); - - /* ************************************************************************* */ - Pose2 prior(const Pose2& x, boost::optional H) { - if (H) *H = I; - return x; - } - - /* ************************************************************************* */ - Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional H1, - boost::optional H2) { - return x1.between(x2, H1, H2); - } - -/* ************************************************************************* */ - -} // namespace simulated2DOriented - diff --git a/tests/simulated2DOriented.h b/tests/simulated2DOriented.h index a18ace2e3..b05fb45c1 100644 --- a/tests/simulated2DOriented.h +++ b/tests/simulated2DOriented.h @@ -62,7 +62,10 @@ namespace simulated2DOriented { } /// Prior on a single pose, optional derivative version - Pose2 prior(const Pose2& x, boost::optional H = boost::none); + Pose2 prior(const Pose2& x, boost::optional H = boost::none) { + if (H) *H = gtsam::eye(3); + return x; + } /// odometry between two poses inline Pose2 odo(const Pose2& x1, const Pose2& x2) { @@ -71,7 +74,9 @@ namespace simulated2DOriented { /// odometry between two poses, optional derivative version Pose2 odo(const Pose2& x1, const Pose2& x2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none); + boost::none, boost::optional H2 = boost::none) { + return x1.between(x2, H1, H2); + } /// Unary factor encoding a soft prior on a vector template diff --git a/tests/simulated3D.cpp b/tests/simulated3D.cpp deleted file mode 100644 index c7ba2a9b0..000000000 --- a/tests/simulated3D.cpp +++ /dev/null @@ -1,43 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** -* @file Simulated3D.cpp -* @brief measurement functions and derivatives for simulated 3D robot -* @author Alex Cunningham -**/ - -#include - -namespace gtsam { - -namespace simulated3D { - -Point3 prior (const Point3& x, boost::optional H) { - if (H) *H = eye(3); - return x; -} - -Point3 odo(const Point3& x1, const Point3& x2, - boost::optional H1, boost::optional H2) { - if (H1) *H1 = -1 * eye(3); - if (H2) *H2 = eye(3); - return x2 - x1; -} - -Point3 mea(const Point3& x, const Point3& l, - boost::optional H1, boost::optional H2) { - if (H1) *H1 = -1 * eye(3); - if (H2) *H2 = eye(3); - return l - x; -} - -}} // namespace gtsam::simulated3D diff --git a/tests/simulated3D.h b/tests/simulated3D.h index 8aa0dc469..46945558a 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -38,21 +38,32 @@ namespace simulated3D { /** * Prior on a single pose */ -Point3 prior(const Point3& x, boost::optional H = boost::none); +Point3 prior(const Point3& x, boost::optional H = boost::none) { + if (H) *H = eye(3); + return x; +} /** * odometry between two poses */ Point3 odo(const Point3& x1, const Point3& x2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none); + boost::optional H2 = boost::none) { + if (H1) *H1 = -1 * eye(3); + if (H2) *H2 = eye(3); + return x2 - x1; +} /** * measurement between landmark and pose */ Point3 mea(const Point3& x, const Point3& l, boost::optional H1 = boost::none, - boost::optional H2 = boost::none); + boost::optional H2 = boost::none) { + if (H1) *H1 = -1 * eye(3); + if (H2) *H2 = eye(3); + return l - x; +} /** * A prior factor on a single linear robot pose diff --git a/tests/smallExample.cpp b/tests/smallExample.cpp deleted file mode 100644 index 6074109a8..000000000 --- a/tests/smallExample.cpp +++ /dev/null @@ -1,502 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file smallExample.cpp - * @brief Create small example with two poses and one landmark - * @brief smallExample - * @author Carlos Nieto - * @author Frank dellaert - */ - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include - -#include -#include - -using namespace std; - -namespace gtsam { -namespace example { - - using namespace gtsam::noiseModel; - - typedef boost::shared_ptr shared; - - static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); - static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); - static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); - static SharedDiagonal constraintModel = noiseModel::Constrained::All(2); - - static const Index _l1_=0, _x1_=1, _x2_=2; - static const Index _x_=0, _y_=1, _z_=2; - - // Convenience for named keys - using symbol_shorthand::X; - using symbol_shorthand::L; - - /* ************************************************************************* */ - boost::shared_ptr sharedNonlinearFactorGraph() { - // Create - boost::shared_ptr nlfg( - new Graph); - - // prior on x1 - Point2 mu; - shared f1(new simulated2D::Prior(mu, sigma0_1, X(1))); - nlfg->push_back(f1); - - // odometry between x1 and x2 - Point2 z2(1.5, 0); - shared f2(new simulated2D::Odometry(z2, sigma0_1, X(1), X(2))); - nlfg->push_back(f2); - - // measurement between x1 and l1 - Point2 z3(0, -1); - shared f3(new simulated2D::Measurement(z3, sigma0_2, X(1), L(1))); - nlfg->push_back(f3); - - // measurement between x2 and l1 - Point2 z4(-1.5, -1.); - shared f4(new simulated2D::Measurement(z4, sigma0_2, X(2), L(1))); - nlfg->push_back(f4); - - return nlfg; - } - - /* ************************************************************************* */ - Graph createNonlinearFactorGraph() { - return *sharedNonlinearFactorGraph(); - } - - /* ************************************************************************* */ - Values createValues() { - Values c; - c.insert(X(1), Point2(0.0, 0.0)); - c.insert(X(2), Point2(1.5, 0.0)); - c.insert(L(1), Point2(0.0, -1.0)); - return c; - } - - /* ************************************************************************* */ - VectorValues createVectorValues() { - VectorValues c(vector(3, 2)); - c[_l1_] = Vector_(2, 0.0, -1.0); - c[_x1_] = Vector_(2, 0.0, 0.0); - c[_x2_] = Vector_(2, 1.5, 0.0); - return c; - } - - /* ************************************************************************* */ - boost::shared_ptr sharedNoisyValues() { - boost::shared_ptr c(new Values); - c->insert(X(1), Point2(0.1, 0.1)); - c->insert(X(2), Point2(1.4, 0.2)); - c->insert(L(1), Point2(0.1, -1.1)); - return c; - } - - /* ************************************************************************* */ - Values createNoisyValues() { - return *sharedNoisyValues(); - } - - /* ************************************************************************* */ - VectorValues createCorrectDelta(const Ordering& ordering) { - VectorValues c(vector(3,2)); - c[ordering[L(1)]] = Vector_(2, -0.1, 0.1); - c[ordering[X(1)]] = Vector_(2, -0.1, -0.1); - c[ordering[X(2)]] = Vector_(2, 0.1, -0.2); - return c; - } - - /* ************************************************************************* */ - VectorValues createZeroDelta(const Ordering& ordering) { - VectorValues c(vector(3,2)); - c[ordering[L(1)]] = zero(2); - c[ordering[X(1)]] = zero(2); - c[ordering[X(2)]] = zero(2); - return c; - } - - /* ************************************************************************* */ - GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { - // Create empty graph - GaussianFactorGraph fg; - - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - - // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg.push_back(boost::make_shared(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2)); - - // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg.push_back(boost::make_shared(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2)); - - // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg.push_back(boost::make_shared(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2)); - - // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg.push_back(boost::make_shared(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2)); - - return fg; - } - - /* ************************************************************************* */ - /** create small Chordal Bayes Net x <- y - * x y d - * 1 1 9 - * 1 5 - */ - GaussianBayesNet createSmallGaussianBayesNet() { - Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0); - Matrix R22 = Matrix_(1, 1, 1.0); - Vector d1(1), d2(1); - d1(0) = 9; - d2(0) = 5; - Vector tau(1); - tau(0) = 1.0; - - // define nodes and specify in reverse topological sort (i.e. parents last) - GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12, tau)); - GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22, tau)); - GaussianBayesNet cbn; - cbn.push_back(Px_y); - cbn.push_back(Py); - - return cbn; - } - - /* ************************************************************************* */ - // Some nonlinear functions to optimize - /* ************************************************************************* */ - namespace smallOptimize { - - Point2 h(const Point2& v) { - return Point2(cos(v.x()), sin(v.y())); - } - - Matrix H(const Point2& v) { - return Matrix_(2, 2, - -sin(v.x()), 0.0, - 0.0, cos(v.y())); - } - - struct UnaryFactor: public gtsam::NoiseModelFactor1 { - - Point2 z_; - - UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) : - gtsam::NoiseModelFactor1(model, key), z_(z) { - } - - Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { - if (A) *A = H(x); - return (h(x) - z_).vector(); - } - - }; - - } - - /* ************************************************************************* */ - boost::shared_ptr sharedReallyNonlinearFactorGraph() { - boost::shared_ptr fg(new Graph); - Vector z = Vector_(2, 1.0, 0.0); - double sigma = 0.1; - boost::shared_ptr factor( - new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); - fg->push_back(factor); - return fg; - } - - Graph createReallyNonlinearFactorGraph() { - return *sharedReallyNonlinearFactorGraph(); - } - - /* ************************************************************************* */ - pair createNonlinearSmoother(int T) { - - // Create - Graph nlfg; - Values poses; - - // prior on x1 - Point2 x1(1.0, 0.0); - shared prior(new simulated2D::Prior(x1, sigma1_0, X(1))); - nlfg.push_back(prior); - poses.insert(X(1), x1); - - for (int t = 2; t <= T; t++) { - // odometry between x_t and x_{t-1} - Point2 odo(1.0, 0.0); - shared odometry(new simulated2D::Odometry(odo, sigma1_0, X(t - 1), X(t))); - nlfg.push_back(odometry); - - // measurement on x_t is like perfect GPS - Point2 xt(t, 0); - shared measurement(new simulated2D::Prior(xt, sigma1_0, X(t))); - nlfg.push_back(measurement); - - // initial estimate - poses.insert(X(t), xt); - } - - return make_pair(nlfg, poses); - } - - /* ************************************************************************* */ - pair, Ordering> createSmoother(int T, boost::optional ordering) { - Graph nlfg; - Values poses; - boost::tie(nlfg, poses) = createNonlinearSmoother(T); - - if(!ordering) ordering = *poses.orderingArbitrary(); - return make_pair(*nlfg.linearize(poses, *ordering), *ordering); - } - - /* ************************************************************************* */ - GaussianFactorGraph createSimpleConstraintGraph() { - // create unary factor - // prior on _x_, mean = [1,-1], sigma=0.1 - Matrix Ax = eye(2); - Vector b1(2); - b1(0) = 1.0; - b1(1) = -1.0; - JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); - - // create binary constraint factor - // between _x_ and _y_, that is going to be the only factor on _y_ - // |1 0||x_1| + |-1 0||y_1| = |0| - // |0 1||x_2| | 0 -1||y_2| |0| - Matrix Ax1 = eye(2); - Matrix Ay1 = eye(2) * -1; - Vector b2 = Vector_(2, 0.0, 0.0); - JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, - constraintModel)); - - // construct the graph - GaussianFactorGraph fg; - fg.push_back(f1); - fg.push_back(f2); - - return fg; - } - - /* ************************************************************************* */ - VectorValues createSimpleConstraintValues() { - VectorValues config(vector(2,2)); - Vector v = Vector_(2, 1.0, -1.0); - config[_x_] = v; - config[_y_] = v; - return config; - } - - /* ************************************************************************* */ - GaussianFactorGraph createSingleConstraintGraph() { - // create unary factor - // prior on _x_, mean = [1,-1], sigma=0.1 - Matrix Ax = eye(2); - Vector b1(2); - b1(0) = 1.0; - b1(1) = -1.0; - //GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1)); - JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); - - // create binary constraint factor - // between _x_ and _y_, that is going to be the only factor on _y_ - // |1 2||x_1| + |10 0||y_1| = |1| - // |2 1||x_2| |0 10||y_2| |2| - Matrix Ax1(2, 2); - Ax1(0, 0) = 1.0; - Ax1(0, 1) = 2.0; - Ax1(1, 0) = 2.0; - Ax1(1, 1) = 1.0; - Matrix Ay1 = eye(2) * 10; - Vector b2 = Vector_(2, 1.0, 2.0); - JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, - constraintModel)); - - // construct the graph - GaussianFactorGraph fg; - fg.push_back(f1); - fg.push_back(f2); - - return fg; - } - - /* ************************************************************************* */ - VectorValues createSingleConstraintValues() { - VectorValues config(vector(2,2)); - config[_x_] = Vector_(2, 1.0, -1.0); - config[_y_] = Vector_(2, 0.2, 0.1); - return config; - } - - /* ************************************************************************* */ - GaussianFactorGraph createMultiConstraintGraph() { - // unary factor 1 - Matrix A = eye(2); - Vector b = Vector_(2, -2.0, 2.0); - JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); - - // constraint 1 - Matrix A11(2, 2); - A11(0, 0) = 1.0; - A11(0, 1) = 2.0; - A11(1, 0) = 2.0; - A11(1, 1) = 1.0; - - Matrix A12(2, 2); - A12(0, 0) = 10.0; - A12(0, 1) = 0.0; - A12(1, 0) = 0.0; - A12(1, 1) = 10.0; - - Vector b1(2); - b1(0) = 1.0; - b1(1) = 2.0; - JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1, - constraintModel)); - - // constraint 2 - Matrix A21(2, 2); - A21(0, 0) = 3.0; - A21(0, 1) = 4.0; - A21(1, 0) = -1.0; - A21(1, 1) = -2.0; - - Matrix A22(2, 2); - A22(0, 0) = 1.0; - A22(0, 1) = 1.0; - A22(1, 0) = 1.0; - A22(1, 1) = 2.0; - - Vector b2(2); - b2(0) = 3.0; - b2(1) = 4.0; - JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2, - constraintModel)); - - // construct the graph - GaussianFactorGraph fg; - fg.push_back(lf1); - fg.push_back(lc1); - fg.push_back(lc2); - - return fg; - } - - /* ************************************************************************* */ - VectorValues createMultiConstraintValues() { - VectorValues config(vector(3,2)); - config[_x_] = Vector_(2, -2.0, 2.0); - config[_y_] = Vector_(2, -0.1, 0.4); - config[_z_] = Vector_(2, -4.0, 5.0); - return config; - } - - - /* ************************************************************************* */ - // Create key for simulated planar graph - Symbol key(int x, int y) { - return X(1000*x+y); - } - - /* ************************************************************************* */ - boost::tuple planarGraph(size_t N) { - - // create empty graph - NonlinearFactorGraph nlfg; - - // Create almost hard constraint on x11, sigma=0 will work for PCG not for normal - shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), Isotropic::Sigma(2,1e-3), key(1,1))); - nlfg.push_back(constraint); - - // Create horizontal constraints, 1...N*(N-1) - Point2 z1(1.0, 0.0); // move right - for (size_t x = 1; x < N; x++) - for (size_t y = 1; y <= N; y++) { - shared f(new simulated2D::Odometry(z1, Isotropic::Sigma(2,0.01), key(x, y), key(x + 1, y))); - nlfg.push_back(f); - } - - // Create vertical constraints, N*(N-1)+1..2*N*(N-1) - Point2 z2(0.0, 1.0); // move up - for (size_t x = 1; x <= N; x++) - for (size_t y = 1; y < N; y++) { - shared f(new simulated2D::Odometry(z2, Isotropic::Sigma(2,0.01), key(x, y), key(x, y + 1))); - nlfg.push_back(f); - } - - // Create linearization and ground xtrue config - Values zeros; - for (size_t x = 1; x <= N; x++) - for (size_t y = 1; y <= N; y++) - zeros.insert(key(x, y), Point2()); - Ordering ordering(planarOrdering(N)); - VectorValues xtrue(zeros.dims(ordering)); - for (size_t x = 1; x <= N; x++) - for (size_t y = 1; y <= N; y++) - xtrue[ordering[key(x, y)]] = Point2(x,y).vector(); - - // linearize around zero - boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); - return boost::make_tuple(*gfg, xtrue); - } - - /* ************************************************************************* */ - Ordering planarOrdering(size_t N) { - Ordering ordering; - for (size_t y = N; y >= 1; y--) - for (size_t x = N; x >= 1; x--) - ordering.push_back(key(x, y)); - return ordering; - } - - /* ************************************************************************* */ - pair splitOffPlanarTree(size_t N, - const GaussianFactorGraph& original) { - GaussianFactorGraph T, C; - - // Add the x11 constraint to the tree - T.push_back(original[0]); - - // Add all horizontal constraints to the tree - size_t i = 1; - for (size_t x = 1; x < N; x++) - for (size_t y = 1; y <= N; y++, i++) - T.push_back(original[i]); - - // Add first vertical column of constraints to T, others to C - for (size_t x = 1; x <= N; x++) - for (size_t y = 1; y < N; y++, i++) - if (x == 1) - T.push_back(original[i]); - else - C.push_back(original[i]); - - return make_pair(T, C); - } - -/* ************************************************************************* */ - -} // example -} // namespace gtsam diff --git a/tests/smallExample.h b/tests/smallExample.h index 7652d96aa..595211180 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -25,6 +25,19 @@ #include #include +#include +//#include +//#include +//#include +//#include +//#include +// +//#include +//#include +// +//#include +//#include + namespace gtsam { namespace example { @@ -151,3 +164,524 @@ namespace gtsam { } // example } // gtsam + + +// Implementations +namespace gtsam { +namespace example { + +// using namespace gtsam::noiseModel; + +namespace impl { + typedef boost::shared_ptr shared_nlf; + + static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); + static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); + static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); + static SharedDiagonal constraintModel = noiseModel::Constrained::All(2); + + static const Index _l1_=0, _x1_=1, _x2_=2; + static const Index _x_=0, _y_=1, _z_=2; +} // \namespace impl + + + /* ************************************************************************* */ + boost::shared_ptr sharedNonlinearFactorGraph() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + // Create + boost::shared_ptr nlfg( + new Graph); + + // prior on x1 + Point2 mu; + shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1))); + nlfg->push_back(f1); + + // odometry between x1 and x2 + Point2 z2(1.5, 0); + shared_nlf f2(new simulated2D::Odometry(z2, sigma0_1, X(1), X(2))); + nlfg->push_back(f2); + + // measurement between x1 and l1 + Point2 z3(0, -1); + shared_nlf f3(new simulated2D::Measurement(z3, sigma0_2, X(1), L(1))); + nlfg->push_back(f3); + + // measurement between x2 and l1 + Point2 z4(-1.5, -1.); + shared_nlf f4(new simulated2D::Measurement(z4, sigma0_2, X(2), L(1))); + nlfg->push_back(f4); + + return nlfg; + } + + /* ************************************************************************* */ + Graph createNonlinearFactorGraph() { + return *sharedNonlinearFactorGraph(); + } + + /* ************************************************************************* */ + Values createValues() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + Values c; + c.insert(X(1), Point2(0.0, 0.0)); + c.insert(X(2), Point2(1.5, 0.0)); + c.insert(L(1), Point2(0.0, -1.0)); + return c; + } + + /* ************************************************************************* */ + VectorValues createVectorValues() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValues c(std::vector(3, 2)); + c[_l1_] = Vector_(2, 0.0, -1.0); + c[_x1_] = Vector_(2, 0.0, 0.0); + c[_x2_] = Vector_(2, 1.5, 0.0); + return c; + } + + /* ************************************************************************* */ + boost::shared_ptr sharedNoisyValues() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + boost::shared_ptr c(new Values); + c->insert(X(1), Point2(0.1, 0.1)); + c->insert(X(2), Point2(1.4, 0.2)); + c->insert(L(1), Point2(0.1, -1.1)); + return c; + } + + /* ************************************************************************* */ + Values createNoisyValues() { + return *sharedNoisyValues(); + } + + /* ************************************************************************* */ + VectorValues createCorrectDelta(const Ordering& ordering) { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValues c(std::vector(3,2)); + c[ordering[L(1)]] = Vector_(2, -0.1, 0.1); + c[ordering[X(1)]] = Vector_(2, -0.1, -0.1); + c[ordering[X(2)]] = Vector_(2, 0.1, -0.2); + return c; + } + + /* ************************************************************************* */ + VectorValues createZeroDelta(const Ordering& ordering) { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValues c(std::vector(3,2)); + c[ordering[L(1)]] = zero(2); + c[ordering[X(1)]] = zero(2); + c[ordering[X(2)]] = zero(2); + return c; + } + + /* ************************************************************************* */ + GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + // Create empty graph + GaussianFactorGraph fg; + + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + + // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] + fg.push_back(boost::make_shared(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2)); + + // odometry between x1 and x2: x2-x1=[0.2;-0.1] + fg.push_back(boost::make_shared(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2)); + + // measurement between x1 and l1: l1-x1=[0.0;0.2] + fg.push_back(boost::make_shared(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2)); + + // measurement between x2 and l1: l1-x2=[-0.2;0.3] + fg.push_back(boost::make_shared(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2)); + + return fg; + } + + /* ************************************************************************* */ + /** create small Chordal Bayes Net x <- y + * x y d + * 1 1 9 + * 1 5 + */ + GaussianBayesNet createSmallGaussianBayesNet() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0); + Matrix R22 = Matrix_(1, 1, 1.0); + Vector d1(1), d2(1); + d1(0) = 9; + d2(0) = 5; + Vector tau(1); + tau(0) = 1.0; + + // define nodes and specify in reverse topological sort (i.e. parents last) + GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12, tau)); + GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22, tau)); + GaussianBayesNet cbn; + cbn.push_back(Px_y); + cbn.push_back(Py); + + return cbn; + } + + /* ************************************************************************* */ + // Some nonlinear functions to optimize + /* ************************************************************************* */ + namespace smallOptimize { + + Point2 h(const Point2& v) { + return Point2(cos(v.x()), sin(v.y())); + } + + Matrix H(const Point2& v) { + return Matrix_(2, 2, + -sin(v.x()), 0.0, + 0.0, cos(v.y())); + } + + struct UnaryFactor: public gtsam::NoiseModelFactor1 { + + Point2 z_; + + UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) : + gtsam::NoiseModelFactor1(model, key), z_(z) { + } + + Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { + if (A) *A = H(x); + return (h(x) - z_).vector(); + } + + }; + + } + + /* ************************************************************************* */ + boost::shared_ptr sharedReallyNonlinearFactorGraph() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + boost::shared_ptr fg(new Graph); + Vector z = Vector_(2, 1.0, 0.0); + double sigma = 0.1; + boost::shared_ptr factor( + new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); + fg->push_back(factor); + return fg; + } + + Graph createReallyNonlinearFactorGraph() { + return *sharedReallyNonlinearFactorGraph(); + } + + /* ************************************************************************* */ + std::pair createNonlinearSmoother(int T) { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + + // Create + Graph nlfg; + Values poses; + + // prior on x1 + Point2 x1(1.0, 0.0); + shared_nlf prior(new simulated2D::Prior(x1, sigma1_0, X(1))); + nlfg.push_back(prior); + poses.insert(X(1), x1); + + for (int t = 2; t <= T; t++) { + // odometry between x_t and x_{t-1} + Point2 odo(1.0, 0.0); + shared_nlf odometry(new simulated2D::Odometry(odo, sigma1_0, X(t - 1), X(t))); + nlfg.push_back(odometry); + + // measurement on x_t is like perfect GPS + Point2 xt(t, 0); + shared_nlf measurement(new simulated2D::Prior(xt, sigma1_0, X(t))); + nlfg.push_back(measurement); + + // initial estimate + poses.insert(X(t), xt); + } + + return std::make_pair(nlfg, poses); + } + + /* ************************************************************************* */ + std::pair, Ordering> createSmoother(int T, boost::optional ordering) { + Graph nlfg; + Values poses; + boost::tie(nlfg, poses) = createNonlinearSmoother(T); + + if(!ordering) ordering = *poses.orderingArbitrary(); + return std::make_pair(*nlfg.linearize(poses, *ordering), *ordering); + } + + /* ************************************************************************* */ + GaussianFactorGraph createSimpleConstraintGraph() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + // create unary factor + // prior on _x_, mean = [1,-1], sigma=0.1 + Matrix Ax = eye(2); + Vector b1(2); + b1(0) = 1.0; + b1(1) = -1.0; + JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); + + // create binary constraint factor + // between _x_ and _y_, that is going to be the only factor on _y_ + // |1 0||x_1| + |-1 0||y_1| = |0| + // |0 1||x_2| | 0 -1||y_2| |0| + Matrix Ax1 = eye(2); + Matrix Ay1 = eye(2) * -1; + Vector b2 = Vector_(2, 0.0, 0.0); + JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, + constraintModel)); + + // construct the graph + GaussianFactorGraph fg; + fg.push_back(f1); + fg.push_back(f2); + + return fg; + } + + /* ************************************************************************* */ + VectorValues createSimpleConstraintValues() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValues config(std::vector(2,2)); + Vector v = Vector_(2, 1.0, -1.0); + config[_x_] = v; + config[_y_] = v; + return config; + } + + /* ************************************************************************* */ + GaussianFactorGraph createSingleConstraintGraph() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + // create unary factor + // prior on _x_, mean = [1,-1], sigma=0.1 + Matrix Ax = eye(2); + Vector b1(2); + b1(0) = 1.0; + b1(1) = -1.0; + //GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1)); + JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); + + // create binary constraint factor + // between _x_ and _y_, that is going to be the only factor on _y_ + // |1 2||x_1| + |10 0||y_1| = |1| + // |2 1||x_2| |0 10||y_2| |2| + Matrix Ax1(2, 2); + Ax1(0, 0) = 1.0; + Ax1(0, 1) = 2.0; + Ax1(1, 0) = 2.0; + Ax1(1, 1) = 1.0; + Matrix Ay1 = eye(2) * 10; + Vector b2 = Vector_(2, 1.0, 2.0); + JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, + constraintModel)); + + // construct the graph + GaussianFactorGraph fg; + fg.push_back(f1); + fg.push_back(f2); + + return fg; + } + + /* ************************************************************************* */ + VectorValues createSingleConstraintValues() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValues config(std::vector(2,2)); + config[_x_] = Vector_(2, 1.0, -1.0); + config[_y_] = Vector_(2, 0.2, 0.1); + return config; + } + + /* ************************************************************************* */ + GaussianFactorGraph createMultiConstraintGraph() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + // unary factor 1 + Matrix A = eye(2); + Vector b = Vector_(2, -2.0, 2.0); + JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); + + // constraint 1 + Matrix A11(2, 2); + A11(0, 0) = 1.0; + A11(0, 1) = 2.0; + A11(1, 0) = 2.0; + A11(1, 1) = 1.0; + + Matrix A12(2, 2); + A12(0, 0) = 10.0; + A12(0, 1) = 0.0; + A12(1, 0) = 0.0; + A12(1, 1) = 10.0; + + Vector b1(2); + b1(0) = 1.0; + b1(1) = 2.0; + JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1, + constraintModel)); + + // constraint 2 + Matrix A21(2, 2); + A21(0, 0) = 3.0; + A21(0, 1) = 4.0; + A21(1, 0) = -1.0; + A21(1, 1) = -2.0; + + Matrix A22(2, 2); + A22(0, 0) = 1.0; + A22(0, 1) = 1.0; + A22(1, 0) = 1.0; + A22(1, 1) = 2.0; + + Vector b2(2); + b2(0) = 3.0; + b2(1) = 4.0; + JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2, + constraintModel)); + + // construct the graph + GaussianFactorGraph fg; + fg.push_back(lf1); + fg.push_back(lc1); + fg.push_back(lc2); + + return fg; + } + + /* ************************************************************************* */ + VectorValues createMultiConstraintValues() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValues config(std::vector(3,2)); + config[_x_] = Vector_(2, -2.0, 2.0); + config[_y_] = Vector_(2, -0.1, 0.4); + config[_z_] = Vector_(2, -4.0, 5.0); + return config; + } + + /* ************************************************************************* */ + // Create key for simulated planar graph + namespace impl { + Symbol key(int x, int y) { + using symbol_shorthand::X; + return X(1000*x+y); + } + } // \namespace impl + + /* ************************************************************************* */ + boost::tuple planarGraph(size_t N) { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + + // create empty graph + NonlinearFactorGraph nlfg; + + // Create almost hard constraint on x11, sigma=0 will work for PCG not for normal + shared_nlf constraint(new simulated2D::Prior(Point2(1.0, 1.0), noiseModel::Isotropic::Sigma(2,1e-3), key(1,1))); + nlfg.push_back(constraint); + + // Create horizontal constraints, 1...N*(N-1) + Point2 z1(1.0, 0.0); // move right + for (size_t x = 1; x < N; x++) + for (size_t y = 1; y <= N; y++) { + shared_nlf f(new simulated2D::Odometry(z1, noiseModel::Isotropic::Sigma(2,0.01), key(x, y), key(x + 1, y))); + nlfg.push_back(f); + } + + // Create vertical constraints, N*(N-1)+1..2*N*(N-1) + Point2 z2(0.0, 1.0); // move up + for (size_t x = 1; x <= N; x++) + for (size_t y = 1; y < N; y++) { + shared_nlf f(new simulated2D::Odometry(z2, noiseModel::Isotropic::Sigma(2,0.01), key(x, y), key(x, y + 1))); + nlfg.push_back(f); + } + + // Create linearization and ground xtrue config + Values zeros; + for (size_t x = 1; x <= N; x++) + for (size_t y = 1; y <= N; y++) + zeros.insert(key(x, y), Point2()); + Ordering ordering(planarOrdering(N)); + VectorValues xtrue(zeros.dims(ordering)); + for (size_t x = 1; x <= N; x++) + for (size_t y = 1; y <= N; y++) + xtrue[ordering[key(x, y)]] = Point2(x,y).vector(); + + // linearize around zero + boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); + return boost::make_tuple(*gfg, xtrue); + } + + /* ************************************************************************* */ + Ordering planarOrdering(size_t N) { + Ordering ordering; + for (size_t y = N; y >= 1; y--) + for (size_t x = N; x >= 1; x--) + ordering.push_back(impl::key(x, y)); + return ordering; + } + + /* ************************************************************************* */ + std::pair splitOffPlanarTree(size_t N, + const GaussianFactorGraph& original) { + GaussianFactorGraph T, C; + + // Add the x11 constraint to the tree + T.push_back(original[0]); + + // Add all horizontal constraints to the tree + size_t i = 1; + for (size_t x = 1; x < N; x++) + for (size_t y = 1; y <= N; y++, i++) + T.push_back(original[i]); + + // Add first vertical column of constraints to T, others to C + for (size_t x = 1; x <= N; x++) + for (size_t y = 1; y < N; y++, i++) + if (x == 1) + T.push_back(original[i]); + else + C.push_back(original[i]); + + return std::make_pair(T, C); + } + +/* ************************************************************************* */ + +} // \namespace example +} // \namespace gtsam From 37f936d41cec672af8ef68c30c26d4bb9a39f86e Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 11 Jun 2013 14:36:50 +0000 Subject: [PATCH 30/47] Cleanup, whitespace --- tests/smallExample.h | 1146 +++++++++++++++++++++--------------------- 1 file changed, 567 insertions(+), 579 deletions(-) diff --git a/tests/smallExample.h b/tests/smallExample.h index 595211180..e048ed0a2 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -22,147 +22,135 @@ #pragma once #include +#include #include #include -#include -//#include -//#include -//#include -//#include -//#include -// -//#include -//#include -// -//#include -//#include - namespace gtsam { - namespace example { +namespace example { - typedef NonlinearFactorGraph Graph; +typedef NonlinearFactorGraph Graph; - /** - * Create small example for non-linear factor graph - */ - boost::shared_ptr sharedNonlinearFactorGraph(); - Graph createNonlinearFactorGraph(); +/** + * Create small example for non-linear factor graph + */ +boost::shared_ptr sharedNonlinearFactorGraph(); +Graph createNonlinearFactorGraph(); - /** - * Create values structure to go with it - * The ground truth values structure for the example above - */ - Values createValues(); +/** + * Create values structure to go with it + * The ground truth values structure for the example above + */ +Values createValues(); - /** Vector Values equivalent */ - VectorValues createVectorValues(); +/** Vector Values equivalent */ +VectorValues createVectorValues(); - /** - * create a noisy values structure for a nonlinear factor graph - */ - boost::shared_ptr sharedNoisyValues(); - Values createNoisyValues(); +/** + * create a noisy values structure for a nonlinear factor graph + */ +boost::shared_ptr sharedNoisyValues(); +Values createNoisyValues(); - /** - * Zero delta config - */ - VectorValues createZeroDelta(const Ordering& ordering); +/** + * Zero delta config + */ +VectorValues createZeroDelta(const Ordering& ordering); - /** - * Delta config that, when added to noisyValues, returns the ground truth - */ - VectorValues createCorrectDelta(const Ordering& ordering); +/** + * Delta config that, when added to noisyValues, returns the ground truth + */ +VectorValues createCorrectDelta(const Ordering& ordering); - /** - * create a linear factor graph - * The non-linear graph above evaluated at NoisyValues - */ - GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering); +/** + * create a linear factor graph + * The non-linear graph above evaluated at NoisyValues + */ +GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering); - /** - * create small Chordal Bayes Net x <- y - */ - GaussianBayesNet createSmallGaussianBayesNet(); +/** + * create small Chordal Bayes Net x <- y + */ +GaussianBayesNet createSmallGaussianBayesNet(); - /** - * Create really non-linear factor graph (cos/sin) - */ - boost::shared_ptr - sharedReallyNonlinearFactorGraph(); - Graph createReallyNonlinearFactorGraph(); +/** + * Create really non-linear factor graph (cos/sin) + */ +boost::shared_ptr +sharedReallyNonlinearFactorGraph(); +Graph createReallyNonlinearFactorGraph(); - /** - * Create a full nonlinear smoother - * @param T number of time-steps - */ - std::pair createNonlinearSmoother(int T); +/** + * Create a full nonlinear smoother + * @param T number of time-steps + */ +std::pair createNonlinearSmoother(int T); - /** - * Create a Kalman smoother by linearizing a non-linear factor graph - * @param T number of time-steps - */ - std::pair, Ordering> createSmoother(int T, boost::optional ordering = boost::none); +/** + * Create a Kalman smoother by linearizing a non-linear factor graph + * @param T number of time-steps + */ +std::pair, Ordering> createSmoother(int T, boost::optional ordering = boost::none); - /* ******************************************************* */ - // Linear Constrained Examples - /* ******************************************************* */ +/* ******************************************************* */ +// Linear Constrained Examples +/* ******************************************************* */ - /** - * Creates a simple constrained graph with one linear factor and - * one binary equality constraint that sets x = y - */ - GaussianFactorGraph createSimpleConstraintGraph(); - VectorValues createSimpleConstraintValues(); +/** + * Creates a simple constrained graph with one linear factor and + * one binary equality constraint that sets x = y + */ +GaussianFactorGraph createSimpleConstraintGraph(); +VectorValues createSimpleConstraintValues(); - /** - * Creates a simple constrained graph with one linear factor and - * one binary constraint. - */ - GaussianFactorGraph createSingleConstraintGraph(); - VectorValues createSingleConstraintValues(); +/** + * Creates a simple constrained graph with one linear factor and + * one binary constraint. + */ +GaussianFactorGraph createSingleConstraintGraph(); +VectorValues createSingleConstraintValues(); - /** - * Creates a constrained graph with a linear factor and two - * binary constraints that share a node - */ - GaussianFactorGraph createMultiConstraintGraph(); - VectorValues createMultiConstraintValues(); +/** + * Creates a constrained graph with a linear factor and two + * binary constraints that share a node + */ +GaussianFactorGraph createMultiConstraintGraph(); +VectorValues createMultiConstraintValues(); - /* ******************************************************* */ - // Planar graph with easy subtree for SubgraphPreconditioner - /* ******************************************************* */ +/* ******************************************************* */ +// Planar graph with easy subtree for SubgraphPreconditioner +/* ******************************************************* */ - /* - * Create factor graph with N^2 nodes, for example for N=3 - * x13-x23-x33 - * | | | - * x12-x22-x32 - * | | | - * -x11-x21-x31 - * with x11 clamped at (1,1), and others related by 2D odometry. - */ - boost::tuple planarGraph(size_t N); +/* + * Create factor graph with N^2 nodes, for example for N=3 + * x13-x23-x33 + * | | | + * x12-x22-x32 + * | | | + * -x11-x21-x31 + * with x11 clamped at (1,1), and others related by 2D odometry. + */ +boost::tuple planarGraph(size_t N); - /* - * Create canonical ordering for planar graph that also works for tree - * With x11 the root, e.g. for N=3 - * x33 x23 x13 x32 x22 x12 x31 x21 x11 - */ - Ordering planarOrdering(size_t N); +/* + * Create canonical ordering for planar graph that also works for tree + * With x11 the root, e.g. for N=3 + * x33 x23 x13 x32 x22 x12 x31 x21 x11 + */ +Ordering planarOrdering(size_t N); - /* - * Split graph into tree and loop closing constraints, e.g., with N=3 - * x13-x23-x33 - * | - * x12-x22-x32 - * | - * -x11-x21-x31 - */ - std::pair splitOffPlanarTree( - size_t N, const GaussianFactorGraph& original); +/* + * Split graph into tree and loop closing constraints, e.g., with N=3 + * x13-x23-x33 + * | + * x12-x22-x32 + * | + * -x11-x21-x31 + */ +std::pair splitOffPlanarTree( + size_t N, const GaussianFactorGraph& original); - } // example +} // example } // gtsam @@ -173,513 +161,513 @@ namespace example { // using namespace gtsam::noiseModel; namespace impl { - typedef boost::shared_ptr shared_nlf; +typedef boost::shared_ptr shared_nlf; - static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); - static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); - static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); - static SharedDiagonal constraintModel = noiseModel::Constrained::All(2); +static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); +static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); +static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); +static SharedDiagonal constraintModel = noiseModel::Constrained::All(2); - static const Index _l1_=0, _x1_=1, _x2_=2; - static const Index _x_=0, _y_=1, _z_=2; +static const Index _l1_=0, _x1_=1, _x2_=2; +static const Index _x_=0, _y_=1, _z_=2; } // \namespace impl - /* ************************************************************************* */ - boost::shared_ptr sharedNonlinearFactorGraph() { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - // Create - boost::shared_ptr nlfg( - new Graph); +/* ************************************************************************* */ +boost::shared_ptr sharedNonlinearFactorGraph() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + // Create + boost::shared_ptr nlfg( + new Graph); - // prior on x1 - Point2 mu; - shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1))); - nlfg->push_back(f1); + // prior on x1 + Point2 mu; + shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1))); + nlfg->push_back(f1); - // odometry between x1 and x2 - Point2 z2(1.5, 0); - shared_nlf f2(new simulated2D::Odometry(z2, sigma0_1, X(1), X(2))); - nlfg->push_back(f2); + // odometry between x1 and x2 + Point2 z2(1.5, 0); + shared_nlf f2(new simulated2D::Odometry(z2, sigma0_1, X(1), X(2))); + nlfg->push_back(f2); - // measurement between x1 and l1 - Point2 z3(0, -1); - shared_nlf f3(new simulated2D::Measurement(z3, sigma0_2, X(1), L(1))); - nlfg->push_back(f3); + // measurement between x1 and l1 + Point2 z3(0, -1); + shared_nlf f3(new simulated2D::Measurement(z3, sigma0_2, X(1), L(1))); + nlfg->push_back(f3); - // measurement between x2 and l1 - Point2 z4(-1.5, -1.); - shared_nlf f4(new simulated2D::Measurement(z4, sigma0_2, X(2), L(1))); - nlfg->push_back(f4); + // measurement between x2 and l1 + Point2 z4(-1.5, -1.); + shared_nlf f4(new simulated2D::Measurement(z4, sigma0_2, X(2), L(1))); + nlfg->push_back(f4); - return nlfg; + return nlfg; +} + +/* ************************************************************************* */ +Graph createNonlinearFactorGraph() { + return *sharedNonlinearFactorGraph(); +} + +/* ************************************************************************* */ +Values createValues() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + Values c; + c.insert(X(1), Point2(0.0, 0.0)); + c.insert(X(2), Point2(1.5, 0.0)); + c.insert(L(1), Point2(0.0, -1.0)); + return c; +} + +/* ************************************************************************* */ +VectorValues createVectorValues() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValues c(std::vector(3, 2)); + c[_l1_] = Vector_(2, 0.0, -1.0); + c[_x1_] = Vector_(2, 0.0, 0.0); + c[_x2_] = Vector_(2, 1.5, 0.0); + return c; +} + +/* ************************************************************************* */ +boost::shared_ptr sharedNoisyValues() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + boost::shared_ptr c(new Values); + c->insert(X(1), Point2(0.1, 0.1)); + c->insert(X(2), Point2(1.4, 0.2)); + c->insert(L(1), Point2(0.1, -1.1)); + return c; +} + +/* ************************************************************************* */ +Values createNoisyValues() { + return *sharedNoisyValues(); +} + +/* ************************************************************************* */ +VectorValues createCorrectDelta(const Ordering& ordering) { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValues c(std::vector(3,2)); + c[ordering[L(1)]] = Vector_(2, -0.1, 0.1); + c[ordering[X(1)]] = Vector_(2, -0.1, -0.1); + c[ordering[X(2)]] = Vector_(2, 0.1, -0.2); + return c; +} + +/* ************************************************************************* */ +VectorValues createZeroDelta(const Ordering& ordering) { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValues c(std::vector(3,2)); + c[ordering[L(1)]] = zero(2); + c[ordering[X(1)]] = zero(2); + c[ordering[X(2)]] = zero(2); + return c; +} + +/* ************************************************************************* */ +GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + // Create empty graph + GaussianFactorGraph fg; + + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + + // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] + fg.push_back(boost::make_shared(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2)); + + // odometry between x1 and x2: x2-x1=[0.2;-0.1] + fg.push_back(boost::make_shared(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2)); + + // measurement between x1 and l1: l1-x1=[0.0;0.2] + fg.push_back(boost::make_shared(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2)); + + // measurement between x2 and l1: l1-x2=[-0.2;0.3] + fg.push_back(boost::make_shared(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2)); + + return fg; +} + +/* ************************************************************************* */ +/** create small Chordal Bayes Net x <- y + * x y d + * 1 1 9 + * 1 5 + */ +GaussianBayesNet createSmallGaussianBayesNet() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0); + Matrix R22 = Matrix_(1, 1, 1.0); + Vector d1(1), d2(1); + d1(0) = 9; + d2(0) = 5; + Vector tau(1); + tau(0) = 1.0; + + // define nodes and specify in reverse topological sort (i.e. parents last) + GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12, tau)); + GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22, tau)); + GaussianBayesNet cbn; + cbn.push_back(Px_y); + cbn.push_back(Py); + + return cbn; +} + +/* ************************************************************************* */ +// Some nonlinear functions to optimize +/* ************************************************************************* */ +namespace smallOptimize { + +Point2 h(const Point2& v) { + return Point2(cos(v.x()), sin(v.y())); +} + +Matrix H(const Point2& v) { + return Matrix_(2, 2, + -sin(v.x()), 0.0, + 0.0, cos(v.y())); +} + +struct UnaryFactor: public gtsam::NoiseModelFactor1 { + + Point2 z_; + + UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) : + gtsam::NoiseModelFactor1(model, key), z_(z) { } - /* ************************************************************************* */ - Graph createNonlinearFactorGraph() { - return *sharedNonlinearFactorGraph(); + Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { + if (A) *A = H(x); + return (h(x) - z_).vector(); } - /* ************************************************************************* */ - Values createValues() { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - Values c; - c.insert(X(1), Point2(0.0, 0.0)); - c.insert(X(2), Point2(1.5, 0.0)); - c.insert(L(1), Point2(0.0, -1.0)); - return c; +}; + +} + +/* ************************************************************************* */ +boost::shared_ptr sharedReallyNonlinearFactorGraph() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + boost::shared_ptr fg(new Graph); + Vector z = Vector_(2, 1.0, 0.0); + double sigma = 0.1; + boost::shared_ptr factor( + new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); + fg->push_back(factor); + return fg; +} + +Graph createReallyNonlinearFactorGraph() { + return *sharedReallyNonlinearFactorGraph(); +} + +/* ************************************************************************* */ +std::pair createNonlinearSmoother(int T) { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + + // Create + Graph nlfg; + Values poses; + + // prior on x1 + Point2 x1(1.0, 0.0); + shared_nlf prior(new simulated2D::Prior(x1, sigma1_0, X(1))); + nlfg.push_back(prior); + poses.insert(X(1), x1); + + for (int t = 2; t <= T; t++) { + // odometry between x_t and x_{t-1} + Point2 odo(1.0, 0.0); + shared_nlf odometry(new simulated2D::Odometry(odo, sigma1_0, X(t - 1), X(t))); + nlfg.push_back(odometry); + + // measurement on x_t is like perfect GPS + Point2 xt(t, 0); + shared_nlf measurement(new simulated2D::Prior(xt, sigma1_0, X(t))); + nlfg.push_back(measurement); + + // initial estimate + poses.insert(X(t), xt); } - /* ************************************************************************* */ - VectorValues createVectorValues() { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - VectorValues c(std::vector(3, 2)); - c[_l1_] = Vector_(2, 0.0, -1.0); - c[_x1_] = Vector_(2, 0.0, 0.0); - c[_x2_] = Vector_(2, 1.5, 0.0); - return c; - } + return std::make_pair(nlfg, poses); +} - /* ************************************************************************* */ - boost::shared_ptr sharedNoisyValues() { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - boost::shared_ptr c(new Values); - c->insert(X(1), Point2(0.1, 0.1)); - c->insert(X(2), Point2(1.4, 0.2)); - c->insert(L(1), Point2(0.1, -1.1)); - return c; - } +/* ************************************************************************* */ +std::pair, Ordering> createSmoother(int T, boost::optional ordering) { + Graph nlfg; + Values poses; + boost::tie(nlfg, poses) = createNonlinearSmoother(T); - /* ************************************************************************* */ - Values createNoisyValues() { - return *sharedNoisyValues(); - } + if(!ordering) ordering = *poses.orderingArbitrary(); + return std::make_pair(*nlfg.linearize(poses, *ordering), *ordering); +} - /* ************************************************************************* */ - VectorValues createCorrectDelta(const Ordering& ordering) { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - VectorValues c(std::vector(3,2)); - c[ordering[L(1)]] = Vector_(2, -0.1, 0.1); - c[ordering[X(1)]] = Vector_(2, -0.1, -0.1); - c[ordering[X(2)]] = Vector_(2, 0.1, -0.2); - return c; - } +/* ************************************************************************* */ +GaussianFactorGraph createSimpleConstraintGraph() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + // create unary factor + // prior on _x_, mean = [1,-1], sigma=0.1 + Matrix Ax = eye(2); + Vector b1(2); + b1(0) = 1.0; + b1(1) = -1.0; + JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); - /* ************************************************************************* */ - VectorValues createZeroDelta(const Ordering& ordering) { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - VectorValues c(std::vector(3,2)); - c[ordering[L(1)]] = zero(2); - c[ordering[X(1)]] = zero(2); - c[ordering[X(2)]] = zero(2); - return c; - } + // create binary constraint factor + // between _x_ and _y_, that is going to be the only factor on _y_ + // |1 0||x_1| + |-1 0||y_1| = |0| + // |0 1||x_2| | 0 -1||y_2| |0| + Matrix Ax1 = eye(2); + Matrix Ay1 = eye(2) * -1; + Vector b2 = Vector_(2, 0.0, 0.0); + JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, + constraintModel)); - /* ************************************************************************* */ - GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - // Create empty graph - GaussianFactorGraph fg; + // construct the graph + GaussianFactorGraph fg; + fg.push_back(f1); + fg.push_back(f2); - SharedDiagonal unit2 = noiseModel::Unit::Create(2); + return fg; +} - // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg.push_back(boost::make_shared(ordering[X(1)], 10*eye(2), -1.0*ones(2), unit2)); +/* ************************************************************************* */ +VectorValues createSimpleConstraintValues() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValues config(std::vector(2,2)); + Vector v = Vector_(2, 1.0, -1.0); + config[_x_] = v; + config[_y_] = v; + return config; +} - // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg.push_back(boost::make_shared(ordering[X(1)], -10*eye(2),ordering[X(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2)); +/* ************************************************************************* */ +GaussianFactorGraph createSingleConstraintGraph() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + // create unary factor + // prior on _x_, mean = [1,-1], sigma=0.1 + Matrix Ax = eye(2); + Vector b1(2); + b1(0) = 1.0; + b1(1) = -1.0; + //GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1)); + JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); - // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg.push_back(boost::make_shared(ordering[X(1)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2)); + // create binary constraint factor + // between _x_ and _y_, that is going to be the only factor on _y_ + // |1 2||x_1| + |10 0||y_1| = |1| + // |2 1||x_2| |0 10||y_2| |2| + Matrix Ax1(2, 2); + Ax1(0, 0) = 1.0; + Ax1(0, 1) = 2.0; + Ax1(1, 0) = 2.0; + Ax1(1, 1) = 1.0; + Matrix Ay1 = eye(2) * 10; + Vector b2 = Vector_(2, 1.0, 2.0); + JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, + constraintModel)); - // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg.push_back(boost::make_shared(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2)); + // construct the graph + GaussianFactorGraph fg; + fg.push_back(f1); + fg.push_back(f2); - return fg; - } + return fg; +} - /* ************************************************************************* */ - /** create small Chordal Bayes Net x <- y - * x y d - * 1 1 9 - * 1 5 - */ - GaussianBayesNet createSmallGaussianBayesNet() { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0); - Matrix R22 = Matrix_(1, 1, 1.0); - Vector d1(1), d2(1); - d1(0) = 9; - d2(0) = 5; - Vector tau(1); - tau(0) = 1.0; +/* ************************************************************************* */ +VectorValues createSingleConstraintValues() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValues config(std::vector(2,2)); + config[_x_] = Vector_(2, 1.0, -1.0); + config[_y_] = Vector_(2, 0.2, 0.1); + return config; +} - // define nodes and specify in reverse topological sort (i.e. parents last) - GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11, _y_, S12, tau)); - GaussianConditional::shared_ptr Py(new GaussianConditional(_y_, d2, R22, tau)); - GaussianBayesNet cbn; - cbn.push_back(Px_y); - cbn.push_back(Py); +/* ************************************************************************* */ +GaussianFactorGraph createMultiConstraintGraph() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + // unary factor 1 + Matrix A = eye(2); + Vector b = Vector_(2, -2.0, 2.0); + JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); - return cbn; - } + // constraint 1 + Matrix A11(2, 2); + A11(0, 0) = 1.0; + A11(0, 1) = 2.0; + A11(1, 0) = 2.0; + A11(1, 1) = 1.0; - /* ************************************************************************* */ - // Some nonlinear functions to optimize - /* ************************************************************************* */ - namespace smallOptimize { + Matrix A12(2, 2); + A12(0, 0) = 10.0; + A12(0, 1) = 0.0; + A12(1, 0) = 0.0; + A12(1, 1) = 10.0; - Point2 h(const Point2& v) { - return Point2(cos(v.x()), sin(v.y())); + Vector b1(2); + b1(0) = 1.0; + b1(1) = 2.0; + JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1, + constraintModel)); + + // constraint 2 + Matrix A21(2, 2); + A21(0, 0) = 3.0; + A21(0, 1) = 4.0; + A21(1, 0) = -1.0; + A21(1, 1) = -2.0; + + Matrix A22(2, 2); + A22(0, 0) = 1.0; + A22(0, 1) = 1.0; + A22(1, 0) = 1.0; + A22(1, 1) = 2.0; + + Vector b2(2); + b2(0) = 3.0; + b2(1) = 4.0; + JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2, + constraintModel)); + + // construct the graph + GaussianFactorGraph fg; + fg.push_back(lf1); + fg.push_back(lc1); + fg.push_back(lc2); + + return fg; +} + +/* ************************************************************************* */ +VectorValues createMultiConstraintValues() { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + VectorValues config(std::vector(3,2)); + config[_x_] = Vector_(2, -2.0, 2.0); + config[_y_] = Vector_(2, -0.1, 0.4); + config[_z_] = Vector_(2, -4.0, 5.0); + return config; +} + +/* ************************************************************************* */ +// Create key for simulated planar graph +namespace impl { +Symbol key(int x, int y) { + using symbol_shorthand::X; + return X(1000*x+y); +} +} // \namespace impl + +/* ************************************************************************* */ +boost::tuple planarGraph(size_t N) { + using namespace impl; + using symbol_shorthand::X; + using symbol_shorthand::L; + + // create empty graph + NonlinearFactorGraph nlfg; + + // Create almost hard constraint on x11, sigma=0 will work for PCG not for normal + shared_nlf constraint(new simulated2D::Prior(Point2(1.0, 1.0), noiseModel::Isotropic::Sigma(2,1e-3), key(1,1))); + nlfg.push_back(constraint); + + // Create horizontal constraints, 1...N*(N-1) + Point2 z1(1.0, 0.0); // move right + for (size_t x = 1; x < N; x++) + for (size_t y = 1; y <= N; y++) { + shared_nlf f(new simulated2D::Odometry(z1, noiseModel::Isotropic::Sigma(2,0.01), key(x, y), key(x + 1, y))); + nlfg.push_back(f); } - Matrix H(const Point2& v) { - return Matrix_(2, 2, - -sin(v.x()), 0.0, - 0.0, cos(v.y())); + // Create vertical constraints, N*(N-1)+1..2*N*(N-1) + Point2 z2(0.0, 1.0); // move up + for (size_t x = 1; x <= N; x++) + for (size_t y = 1; y < N; y++) { + shared_nlf f(new simulated2D::Odometry(z2, noiseModel::Isotropic::Sigma(2,0.01), key(x, y), key(x, y + 1))); + nlfg.push_back(f); } - struct UnaryFactor: public gtsam::NoiseModelFactor1 { + // Create linearization and ground xtrue config + Values zeros; + for (size_t x = 1; x <= N; x++) + for (size_t y = 1; y <= N; y++) + zeros.insert(key(x, y), Point2()); + Ordering ordering(planarOrdering(N)); + VectorValues xtrue(zeros.dims(ordering)); + for (size_t x = 1; x <= N; x++) + for (size_t y = 1; y <= N; y++) + xtrue[ordering[key(x, y)]] = Point2(x,y).vector(); - Point2 z_; + // linearize around zero + boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); + return boost::make_tuple(*gfg, xtrue); +} - UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) : - gtsam::NoiseModelFactor1(model, key), z_(z) { - } +/* ************************************************************************* */ +Ordering planarOrdering(size_t N) { + Ordering ordering; + for (size_t y = N; y >= 1; y--) + for (size_t x = N; x >= 1; x--) + ordering.push_back(impl::key(x, y)); + return ordering; +} - Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { - if (A) *A = H(x); - return (h(x) - z_).vector(); - } +/* ************************************************************************* */ +std::pair splitOffPlanarTree(size_t N, + const GaussianFactorGraph& original) { + GaussianFactorGraph T, C; - }; + // Add the x11 constraint to the tree + T.push_back(original[0]); - } + // Add all horizontal constraints to the tree + size_t i = 1; + for (size_t x = 1; x < N; x++) + for (size_t y = 1; y <= N; y++, i++) + T.push_back(original[i]); - /* ************************************************************************* */ - boost::shared_ptr sharedReallyNonlinearFactorGraph() { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - boost::shared_ptr fg(new Graph); - Vector z = Vector_(2, 1.0, 0.0); - double sigma = 0.1; - boost::shared_ptr factor( - new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); - fg->push_back(factor); - return fg; - } - - Graph createReallyNonlinearFactorGraph() { - return *sharedReallyNonlinearFactorGraph(); - } - - /* ************************************************************************* */ - std::pair createNonlinearSmoother(int T) { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - - // Create - Graph nlfg; - Values poses; - - // prior on x1 - Point2 x1(1.0, 0.0); - shared_nlf prior(new simulated2D::Prior(x1, sigma1_0, X(1))); - nlfg.push_back(prior); - poses.insert(X(1), x1); - - for (int t = 2; t <= T; t++) { - // odometry between x_t and x_{t-1} - Point2 odo(1.0, 0.0); - shared_nlf odometry(new simulated2D::Odometry(odo, sigma1_0, X(t - 1), X(t))); - nlfg.push_back(odometry); - - // measurement on x_t is like perfect GPS - Point2 xt(t, 0); - shared_nlf measurement(new simulated2D::Prior(xt, sigma1_0, X(t))); - nlfg.push_back(measurement); - - // initial estimate - poses.insert(X(t), xt); - } - - return std::make_pair(nlfg, poses); - } - - /* ************************************************************************* */ - std::pair, Ordering> createSmoother(int T, boost::optional ordering) { - Graph nlfg; - Values poses; - boost::tie(nlfg, poses) = createNonlinearSmoother(T); - - if(!ordering) ordering = *poses.orderingArbitrary(); - return std::make_pair(*nlfg.linearize(poses, *ordering), *ordering); - } - - /* ************************************************************************* */ - GaussianFactorGraph createSimpleConstraintGraph() { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - // create unary factor - // prior on _x_, mean = [1,-1], sigma=0.1 - Matrix Ax = eye(2); - Vector b1(2); - b1(0) = 1.0; - b1(1) = -1.0; - JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); - - // create binary constraint factor - // between _x_ and _y_, that is going to be the only factor on _y_ - // |1 0||x_1| + |-1 0||y_1| = |0| - // |0 1||x_2| | 0 -1||y_2| |0| - Matrix Ax1 = eye(2); - Matrix Ay1 = eye(2) * -1; - Vector b2 = Vector_(2, 0.0, 0.0); - JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, - constraintModel)); - - // construct the graph - GaussianFactorGraph fg; - fg.push_back(f1); - fg.push_back(f2); - - return fg; - } - - /* ************************************************************************* */ - VectorValues createSimpleConstraintValues() { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - VectorValues config(std::vector(2,2)); - Vector v = Vector_(2, 1.0, -1.0); - config[_x_] = v; - config[_y_] = v; - return config; - } - - /* ************************************************************************* */ - GaussianFactorGraph createSingleConstraintGraph() { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - // create unary factor - // prior on _x_, mean = [1,-1], sigma=0.1 - Matrix Ax = eye(2); - Vector b1(2); - b1(0) = 1.0; - b1(1) = -1.0; - //GaussianFactor::shared_ptr f1(new JacobianFactor(_x_, sigma0_1->Whiten(Ax), sigma0_1->whiten(b1), sigma0_1)); - JacobianFactor::shared_ptr f1(new JacobianFactor(_x_, Ax, b1, sigma0_1)); - - // create binary constraint factor - // between _x_ and _y_, that is going to be the only factor on _y_ - // |1 2||x_1| + |10 0||y_1| = |1| - // |2 1||x_2| |0 10||y_2| |2| - Matrix Ax1(2, 2); - Ax1(0, 0) = 1.0; - Ax1(0, 1) = 2.0; - Ax1(1, 0) = 2.0; - Ax1(1, 1) = 1.0; - Matrix Ay1 = eye(2) * 10; - Vector b2 = Vector_(2, 1.0, 2.0); - JacobianFactor::shared_ptr f2(new JacobianFactor(_x_, Ax1, _y_, Ay1, b2, - constraintModel)); - - // construct the graph - GaussianFactorGraph fg; - fg.push_back(f1); - fg.push_back(f2); - - return fg; - } - - /* ************************************************************************* */ - VectorValues createSingleConstraintValues() { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - VectorValues config(std::vector(2,2)); - config[_x_] = Vector_(2, 1.0, -1.0); - config[_y_] = Vector_(2, 0.2, 0.1); - return config; - } - - /* ************************************************************************* */ - GaussianFactorGraph createMultiConstraintGraph() { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - // unary factor 1 - Matrix A = eye(2); - Vector b = Vector_(2, -2.0, 2.0); - JacobianFactor::shared_ptr lf1(new JacobianFactor(_x_, A, b, sigma0_1)); - - // constraint 1 - Matrix A11(2, 2); - A11(0, 0) = 1.0; - A11(0, 1) = 2.0; - A11(1, 0) = 2.0; - A11(1, 1) = 1.0; - - Matrix A12(2, 2); - A12(0, 0) = 10.0; - A12(0, 1) = 0.0; - A12(1, 0) = 0.0; - A12(1, 1) = 10.0; - - Vector b1(2); - b1(0) = 1.0; - b1(1) = 2.0; - JacobianFactor::shared_ptr lc1(new JacobianFactor(_x_, A11, _y_, A12, b1, - constraintModel)); - - // constraint 2 - Matrix A21(2, 2); - A21(0, 0) = 3.0; - A21(0, 1) = 4.0; - A21(1, 0) = -1.0; - A21(1, 1) = -2.0; - - Matrix A22(2, 2); - A22(0, 0) = 1.0; - A22(0, 1) = 1.0; - A22(1, 0) = 1.0; - A22(1, 1) = 2.0; - - Vector b2(2); - b2(0) = 3.0; - b2(1) = 4.0; - JacobianFactor::shared_ptr lc2(new JacobianFactor(_x_, A21, _z_, A22, b2, - constraintModel)); - - // construct the graph - GaussianFactorGraph fg; - fg.push_back(lf1); - fg.push_back(lc1); - fg.push_back(lc2); - - return fg; - } - - /* ************************************************************************* */ - VectorValues createMultiConstraintValues() { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - VectorValues config(std::vector(3,2)); - config[_x_] = Vector_(2, -2.0, 2.0); - config[_y_] = Vector_(2, -0.1, 0.4); - config[_z_] = Vector_(2, -4.0, 5.0); - return config; - } - - /* ************************************************************************* */ - // Create key for simulated planar graph - namespace impl { - Symbol key(int x, int y) { - using symbol_shorthand::X; - return X(1000*x+y); - } - } // \namespace impl - - /* ************************************************************************* */ - boost::tuple planarGraph(size_t N) { - using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; - - // create empty graph - NonlinearFactorGraph nlfg; - - // Create almost hard constraint on x11, sigma=0 will work for PCG not for normal - shared_nlf constraint(new simulated2D::Prior(Point2(1.0, 1.0), noiseModel::Isotropic::Sigma(2,1e-3), key(1,1))); - nlfg.push_back(constraint); - - // Create horizontal constraints, 1...N*(N-1) - Point2 z1(1.0, 0.0); // move right - for (size_t x = 1; x < N; x++) - for (size_t y = 1; y <= N; y++) { - shared_nlf f(new simulated2D::Odometry(z1, noiseModel::Isotropic::Sigma(2,0.01), key(x, y), key(x + 1, y))); - nlfg.push_back(f); - } - - // Create vertical constraints, N*(N-1)+1..2*N*(N-1) - Point2 z2(0.0, 1.0); // move up - for (size_t x = 1; x <= N; x++) - for (size_t y = 1; y < N; y++) { - shared_nlf f(new simulated2D::Odometry(z2, noiseModel::Isotropic::Sigma(2,0.01), key(x, y), key(x, y + 1))); - nlfg.push_back(f); - } - - // Create linearization and ground xtrue config - Values zeros; - for (size_t x = 1; x <= N; x++) - for (size_t y = 1; y <= N; y++) - zeros.insert(key(x, y), Point2()); - Ordering ordering(planarOrdering(N)); - VectorValues xtrue(zeros.dims(ordering)); - for (size_t x = 1; x <= N; x++) - for (size_t y = 1; y <= N; y++) - xtrue[ordering[key(x, y)]] = Point2(x,y).vector(); - - // linearize around zero - boost::shared_ptr gfg = nlfg.linearize(zeros, ordering); - return boost::make_tuple(*gfg, xtrue); - } - - /* ************************************************************************* */ - Ordering planarOrdering(size_t N) { - Ordering ordering; - for (size_t y = N; y >= 1; y--) - for (size_t x = N; x >= 1; x--) - ordering.push_back(impl::key(x, y)); - return ordering; - } - - /* ************************************************************************* */ - std::pair splitOffPlanarTree(size_t N, - const GaussianFactorGraph& original) { - GaussianFactorGraph T, C; - - // Add the x11 constraint to the tree - T.push_back(original[0]); - - // Add all horizontal constraints to the tree - size_t i = 1; - for (size_t x = 1; x < N; x++) - for (size_t y = 1; y <= N; y++, i++) + // Add first vertical column of constraints to T, others to C + for (size_t x = 1; x <= N; x++) + for (size_t y = 1; y < N; y++, i++) + if (x == 1) T.push_back(original[i]); + else + C.push_back(original[i]); - // Add first vertical column of constraints to T, others to C - for (size_t x = 1; x <= N; x++) - for (size_t y = 1; y < N; y++, i++) - if (x == 1) - T.push_back(original[i]); - else - C.push_back(original[i]); - - return std::make_pair(T, C); - } + return std::make_pair(T, C); +} /* ************************************************************************* */ From 0c7182b713e9acb47bb96a2258cb9ac02e3db727 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 11 Jun 2013 14:36:50 +0000 Subject: [PATCH 31/47] Removed unnecessary using statements --- tests/smallExample.h | 22 ---------------------- 1 file changed, 22 deletions(-) diff --git a/tests/smallExample.h b/tests/smallExample.h index e048ed0a2..ea9f30b54 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -212,7 +212,6 @@ Graph createNonlinearFactorGraph() { /* ************************************************************************* */ Values createValues() { - using namespace impl; using symbol_shorthand::X; using symbol_shorthand::L; Values c; @@ -225,8 +224,6 @@ Values createValues() { /* ************************************************************************* */ VectorValues createVectorValues() { using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; VectorValues c(std::vector(3, 2)); c[_l1_] = Vector_(2, 0.0, -1.0); c[_x1_] = Vector_(2, 0.0, 0.0); @@ -236,7 +233,6 @@ VectorValues createVectorValues() { /* ************************************************************************* */ boost::shared_ptr sharedNoisyValues() { - using namespace impl; using symbol_shorthand::X; using symbol_shorthand::L; boost::shared_ptr c(new Values); @@ -253,7 +249,6 @@ Values createNoisyValues() { /* ************************************************************************* */ VectorValues createCorrectDelta(const Ordering& ordering) { - using namespace impl; using symbol_shorthand::X; using symbol_shorthand::L; VectorValues c(std::vector(3,2)); @@ -265,7 +260,6 @@ VectorValues createCorrectDelta(const Ordering& ordering) { /* ************************************************************************* */ VectorValues createZeroDelta(const Ordering& ordering) { - using namespace impl; using symbol_shorthand::X; using symbol_shorthand::L; VectorValues c(std::vector(3,2)); @@ -277,7 +271,6 @@ VectorValues createZeroDelta(const Ordering& ordering) { /* ************************************************************************* */ GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { - using namespace impl; using symbol_shorthand::X; using symbol_shorthand::L; // Create empty graph @@ -308,8 +301,6 @@ GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) { */ GaussianBayesNet createSmallGaussianBayesNet() { using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0); Matrix R22 = Matrix_(1, 1, 1.0); Vector d1(1), d2(1); @@ -362,7 +353,6 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1 { /* ************************************************************************* */ boost::shared_ptr sharedReallyNonlinearFactorGraph() { - using namespace impl; using symbol_shorthand::X; using symbol_shorthand::L; boost::shared_ptr fg(new Graph); @@ -425,8 +415,6 @@ std::pair, Ordering> createSmoother(int T, boost::op /* ************************************************************************* */ GaussianFactorGraph createSimpleConstraintGraph() { using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 Matrix Ax = eye(2); @@ -468,8 +456,6 @@ VectorValues createSimpleConstraintValues() { /* ************************************************************************* */ GaussianFactorGraph createSingleConstraintGraph() { using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; // create unary factor // prior on _x_, mean = [1,-1], sigma=0.1 Matrix Ax = eye(2); @@ -504,8 +490,6 @@ GaussianFactorGraph createSingleConstraintGraph() { /* ************************************************************************* */ VectorValues createSingleConstraintValues() { using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; VectorValues config(std::vector(2,2)); config[_x_] = Vector_(2, 1.0, -1.0); config[_y_] = Vector_(2, 0.2, 0.1); @@ -515,8 +499,6 @@ VectorValues createSingleConstraintValues() { /* ************************************************************************* */ GaussianFactorGraph createMultiConstraintGraph() { using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; // unary factor 1 Matrix A = eye(2); Vector b = Vector_(2, -2.0, 2.0); @@ -572,8 +554,6 @@ GaussianFactorGraph createMultiConstraintGraph() { /* ************************************************************************* */ VectorValues createMultiConstraintValues() { using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; VectorValues config(std::vector(3,2)); config[_x_] = Vector_(2, -2.0, 2.0); config[_y_] = Vector_(2, -0.1, 0.4); @@ -593,8 +573,6 @@ Symbol key(int x, int y) { /* ************************************************************************* */ boost::tuple planarGraph(size_t N) { using namespace impl; - using symbol_shorthand::X; - using symbol_shorthand::L; // create empty graph NonlinearFactorGraph nlfg; From 361682c485fb2a45556def95528447d9bbdf0a82 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 11 Jun 2013 14:36:51 +0000 Subject: [PATCH 32/47] Working on a test for liquefy --- .cproject | 362 +++++++++--------- .../linear/tests/testBayesTreeOperations.cpp | 113 +++++- 2 files changed, 301 insertions(+), 174 deletions(-) diff --git a/.cproject b/.cproject index 872909c06..2145af336 100644 --- a/.cproject +++ b/.cproject @@ -1,7 +1,5 @@ - - - + @@ -309,14 +307,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -343,6 +333,7 @@ make + tests/testBayesTree.run true false @@ -350,6 +341,7 @@ make + testBinaryBayesNet.run true false @@ -397,6 +389,7 @@ make + testSymbolicBayesNet.run true false @@ -404,6 +397,7 @@ make + tests/testSymbolicFactor.run true false @@ -411,6 +405,7 @@ make + testSymbolicFactorGraph.run true false @@ -426,11 +421,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -527,22 +531,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -559,6 +547,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -583,34 +587,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run + -j2 + clean true true true @@ -695,26 +691,34 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run true true true @@ -919,6 +923,14 @@ true true + + make + -j5 + testBayesTreeOperations.run + true + true + true + make -j5 @@ -1073,6 +1085,7 @@ make + testGraph.run true false @@ -1080,6 +1093,7 @@ make + testJunctionTree.run true false @@ -1087,6 +1101,7 @@ make + testSymbolicBayesNetB.run true false @@ -1254,6 +1269,7 @@ make + testErrors.run true false @@ -1299,14 +1315,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1387,6 +1395,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1701,7 +1717,6 @@ make - testSimulated2DOriented.run true false @@ -1741,7 +1756,6 @@ make - testSimulated2D.run true false @@ -1749,7 +1763,6 @@ make - testSimulated3D.run true false @@ -1949,7 +1962,6 @@ make - tests/testGaussianISAM2 true false @@ -1971,102 +1983,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2268,6 +2184,7 @@ cpack + -G DEB true false @@ -2275,6 +2192,7 @@ cpack + -G RPM true false @@ -2282,6 +2200,7 @@ cpack + -G TGZ true false @@ -2289,6 +2208,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2430,34 +2350,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2501,6 +2485,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp b/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp index b1c833f2c..cccd47aac 100644 --- a/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp +++ b/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp @@ -10,6 +10,9 @@ #include #include +#include + +#include using namespace gtsam; @@ -17,8 +20,15 @@ SharedDiagonal model2 = noiseModel::Diagonal::Sigmas(ones(2)); SharedDiagonal model4 = noiseModel::Diagonal::Sigmas(ones(4)); SharedDiagonal model6 = noiseModel::Diagonal::Sigmas(ones(6)); +using namespace std; + +using symbol_shorthand::X; +using symbol_shorthand::L; + +static const double tol = 1e-4; + /* ************************************************************************* */ -TEST( testLinearTools, splitFactor ) { +TEST( testBayesTreeOperations, splitFactor ) { // Build upper-triangular system JacobianFactor initFactor( @@ -120,6 +130,107 @@ TEST_UNSAFE( testLinearTools, splitFactor2 ) { EXPECT(assert_equal(expSplit, actSplit)); } +/* ************************************************************************* */ +// Some numbers that should be consistent among all smoother tests + +//static double sigmax1 = 0.786153, /*sigmax2 = 1.0/1.47292,*/ sigmax3 = 0.671512, sigmax4 = +// 0.669534 /*, sigmax5 = sigmax3, sigmax6 = sigmax2*/, sigmax7 = sigmax1; + +/* ************************************************************************* */ +TEST( testBayesTreeOperations, liquefy ) { + using namespace example; + + // Create smoother with 7 nodes + Ordering ordering; + ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); + GaussianFactorGraph smoother = createSmoother(7, ordering).first; + + // Create the Bayes tree + GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); +// bayesTree.print("Full tree"); + + SharedDiagonal unit6 = noiseModel::Diagonal::Sigmas(Vector_(ones(6))); + SharedDiagonal unit4 = noiseModel::Diagonal::Sigmas(Vector_(ones(4))); + SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector_(ones(2))); + + // Liquefy the tree back into a graph + { + GaussianFactorGraph actGraph = liquefy(bayesTree, false); // Doesn't split conditionals + GaussianFactorGraph expGraph; + + Matrix A12 = Matrix_(6, 2, + 1.73205081, 0.0, + 0.0, 1.73205081, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0, + 0.0, 0.0); + + Matrix A15 = Matrix_(6, 2, + -0.577350269, 0.0, + 0.0, -0.577350269, + 1.47196014, 0.0, + 0.0, 1.47196014, + 0.0, 0.0, + 0.0, 0.0); + + Matrix A16 = Matrix_(6, 2, + -0.577350269, 0.0, + 0.0, -0.577350269, + -0.226455407, 0.0, + 0.0, -0.226455407, + 1.49357599, 0.0, + 0.0, 1.49357599); + expGraph.add(2, A12, 5, A15, 6, A16, zeros(6,1), unit6); + + Matrix A21 = Matrix_(4, 2, + 1.73205081, 0.0, + 0.0, 1.73205081, + 0.0, 0.0, + 0.0, 0.0); + + Matrix A24 = Matrix_(4, 2, + -0.577350269, 0.0, + 0.0, -0.577350269, + 1.47196014, 0.0, + 0.0, 1.47196014); + + Matrix A26 = Matrix_(4, 2, + -0.577350269, 0.0, + 0.0, -0.577350269, + -0.226455407, 0.0, + 0.0, -0.226455407); + + expGraph.add(1, A21, 4, A24, 6, A26, zeros(4,1), unit4); + + Matrix A30 = Matrix_(2, 2, + 1.41421356, 0.0, + 0.0, 1.41421356); + + Matrix A34 = Matrix_(2, 2, + -0.707106781, 0.0, + 0.0, -0.707106781); + + expGraph.add(0, A30, 4, A34, zeros(2,1), unit2); + + Matrix A43 = Matrix_(2, 2, + 1.41421356, 0.0, + 0.0, 1.41421356); + Matrix A45 = Matrix_(2, 2, + -0.707106781, 0.0, + 0.0, -0.707106781); + + expGraph.add(3, A43, 5, A45, zeros(2,1), unit2); + + EXPECT(assert_equal(expGraph, actGraph)); + LONGS_EQUAL(4, actGraph.size()); + EXPECT(assert_equal(*expGraph.at(0), *actGraph.at(0))); + EXPECT(assert_equal(*expGraph.at(1), *actGraph.at(1))); + EXPECT(assert_equal(*expGraph.at(2), *actGraph.at(2))); + EXPECT(assert_equal(*expGraph.at(3), *actGraph.at(3))); + } +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 83a1483e9f686c7e59586fc8dcb13b41e420d076 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 11 Jun 2013 14:36:52 +0000 Subject: [PATCH 33/47] Adding unit tests and a bugfix for liquefying bayes tree function --- gtsam_unstable/linear/bayesTreeOperations.cpp | 2 +- .../linear/tests/testBayesTreeOperations.cpp | 154 +++++++++++++++++- 2 files changed, 147 insertions(+), 9 deletions(-) diff --git a/gtsam_unstable/linear/bayesTreeOperations.cpp b/gtsam_unstable/linear/bayesTreeOperations.cpp index e889b3cbb..99daa1bb4 100644 --- a/gtsam_unstable/linear/bayesTreeOperations.cpp +++ b/gtsam_unstable/linear/bayesTreeOperations.cpp @@ -48,7 +48,7 @@ GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor) { JacobianFactor::const_iterator rowIt, colIt; const size_t totalRows = jf->rows(); size_t rowsRemaining = totalRows; - for (rowIt = jf->begin(); rowIt != jf->end(); ++rowIt) { + for (rowIt = jf->begin(); rowIt != jf->end() && rowsRemaining > 0; ++rowIt) { // get dim of current variable size_t varDim = jf->getDim(rowIt); size_t startRow = totalRows - rowsRemaining; diff --git a/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp b/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp index cccd47aac..efc5bba55 100644 --- a/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp +++ b/gtsam_unstable/linear/tests/testBayesTreeOperations.cpp @@ -28,7 +28,7 @@ using symbol_shorthand::L; static const double tol = 1e-4; /* ************************************************************************* */ -TEST( testBayesTreeOperations, splitFactor ) { +TEST( testBayesTreeOperations, splitFactor1 ) { // Build upper-triangular system JacobianFactor initFactor( @@ -68,7 +68,7 @@ TEST( testBayesTreeOperations, splitFactor ) { } /* ************************************************************************* */ -TEST_UNSAFE( testLinearTools, splitFactor2 ) { +TEST( testBayesTreeOperations, splitFactor2 ) { // Build upper-triangular system JacobianFactor initFactor( @@ -130,6 +130,57 @@ TEST_UNSAFE( testLinearTools, splitFactor2 ) { EXPECT(assert_equal(expSplit, actSplit)); } +/* ************************************************************************* */ +TEST( testBayesTreeOperations, splitFactor3 ) { + + // Build upper-triangular system + JacobianFactor initFactor( + 0,Matrix_(4, 2, + 1.0, 2.0, + 0.0, 3.0, + 0.0, 0.0, + 0.0, 0.0), + 1,Matrix_(4, 2, + 1.0, 2.0, + 9.0, 3.0, + 6.0, 8.0, + 0.0, 7.0), + 2,Matrix_(4, 2, + 1.1, 2.2, + 9.1, 3.2, + 6.1, 8.2, + 0.1, 7.2), + Vector_(4, 0.1, 0.2, 0.3, 0.4), + model4); + + GaussianFactorGraph actSplit = splitFactor(initFactor.clone()); + GaussianFactorGraph expSplit; + + expSplit.add( + 0,Matrix_(2, 2, + 1.0, 2.0, + 0.0, 3.0), + 1,Matrix_(2, 2, + 1.0, 2.0, + 9.0, 3.0), + 2,Matrix_(2, 2, + 1.1, 2.2, + 9.1, 3.2), + Vector_(2, 0.1, 0.2), + model2); + expSplit.add( + 1,Matrix_(2, 2, + 6.0, 8.0, + 0.0, 7.0), + 2,Matrix_(2, 2, + 6.1, 8.2, + 0.1, 7.2), + Vector_(2, 0.3, 0.4), + model2); + + EXPECT(assert_equal(expSplit, actSplit)); +} + /* ************************************************************************* */ // Some numbers that should be consistent among all smoother tests @@ -222,12 +273,99 @@ TEST( testBayesTreeOperations, liquefy ) { expGraph.add(3, A43, 5, A45, zeros(2,1), unit2); - EXPECT(assert_equal(expGraph, actGraph)); - LONGS_EQUAL(4, actGraph.size()); - EXPECT(assert_equal(*expGraph.at(0), *actGraph.at(0))); - EXPECT(assert_equal(*expGraph.at(1), *actGraph.at(1))); - EXPECT(assert_equal(*expGraph.at(2), *actGraph.at(2))); - EXPECT(assert_equal(*expGraph.at(3), *actGraph.at(3))); + EXPECT(assert_equal(expGraph, actGraph, tol)); + } + + // Liquefy the tree back into a graph, splitting factors + { + GaussianFactorGraph actGraph = liquefy(bayesTree, true); + GaussianFactorGraph expGraph; + + // Conditional 1 + { + Matrix A12 = Matrix_(2, 2, + 1.73205081, 0.0, + 0.0, 1.73205081); + + Matrix A15 = Matrix_(2, 2, + -0.577350269, 0.0, + 0.0, -0.577350269); + + Matrix A16 = Matrix_(2, 2, + -0.577350269, 0.0, + 0.0, -0.577350269); + expGraph.add(2, A12, 5, A15, 6, A16, zeros(2,1), unit2); + } + + { + Matrix A15 = Matrix_(2, 2, + 1.47196014, 0.0, + 0.0, 1.47196014); + + Matrix A16 = Matrix_(2, 2, + -0.226455407, 0.0, + 0.0, -0.226455407); + expGraph.add(5, A15, 6, A16, zeros(2,1), unit2); + } + + { + Matrix A16 = Matrix_(2, 2, + 1.49357599, 0.0, + 0.0, 1.49357599); + expGraph.add(6, A16, zeros(2,1), unit2); + } + + // Conditional 2 + { + Matrix A21 = Matrix_(2, 2, + 1.73205081, 0.0, + 0.0, 1.73205081); + + Matrix A24 = Matrix_(2, 2, + -0.577350269, 0.0, + 0.0, -0.577350269); + + Matrix A26 = Matrix_(2, 2, + -0.577350269, 0.0, + 0.0, -0.577350269); + + expGraph.add(1, A21, 4, A24, 6, A26, zeros(2,1), unit2); + } + + { + Matrix A24 = Matrix_(2, 2, + 1.47196014, 0.0, + 0.0, 1.47196014); + + Matrix A26 = Matrix_(2, 2, + -0.226455407, 0.0, + 0.0, -0.226455407); + + expGraph.add(4, A24, 6, A26, zeros(2,1), unit2); + } + + // Conditional 3 + Matrix A30 = Matrix_(2, 2, + 1.41421356, 0.0, + 0.0, 1.41421356); + + Matrix A34 = Matrix_(2, 2, + -0.707106781, 0.0, + 0.0, -0.707106781); + + expGraph.add(0, A30, 4, A34, zeros(2,1), unit2); + + // Conditional 4 + Matrix A43 = Matrix_(2, 2, + 1.41421356, 0.0, + 0.0, 1.41421356); + Matrix A45 = Matrix_(2, 2, + -0.707106781, 0.0, + 0.0, -0.707106781); + + expGraph.add(3, A43, 5, A45, zeros(2,1), unit2); + + EXPECT(assert_equal(expGraph, actGraph, tol)); } } From 083e0c849e0c1b4d4a6cce104bc0a367355abc6c Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 11 Jun 2013 14:36:53 +0000 Subject: [PATCH 34/47] Switched to templated version of liquefy() to allow for use with non-default clique types --- gtsam_unstable/linear/bayesTreeOperations.cpp | 20 ------------- gtsam_unstable/linear/bayesTreeOperations.h | 28 ++++++++++++++++--- 2 files changed, 24 insertions(+), 24 deletions(-) diff --git a/gtsam_unstable/linear/bayesTreeOperations.cpp b/gtsam_unstable/linear/bayesTreeOperations.cpp index 99daa1bb4..c2e84dec3 100644 --- a/gtsam_unstable/linear/bayesTreeOperations.cpp +++ b/gtsam_unstable/linear/bayesTreeOperations.cpp @@ -127,26 +127,6 @@ findPathCliques(const GaussianBayesTree::sharedClique& initial) { return result; } -/* ************************************************************************* */ -GaussianFactorGraph liquefy(const GaussianBayesTree::sharedClique& root, bool splitConditionals) { - GaussianFactorGraph result; - if (root && root->conditional()) { - GaussianConditional::shared_ptr conditional = root->conditional(); - if (!splitConditionals) - result.push_back(conditional->toFactor()); - else - result.push_back(splitFactor(conditional->toFactor())); - } - BOOST_FOREACH(const GaussianBayesTree::sharedClique& child, root->children()) - result.push_back(liquefy(child, splitConditionals)); - return result; -} - -/* ************************************************************************* */ -GaussianFactorGraph liquefy(const GaussianBayesTree& bayesTree, bool splitConditionals) { - return liquefy(bayesTree.root(), splitConditionals); -} - /* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam_unstable/linear/bayesTreeOperations.h b/gtsam_unstable/linear/bayesTreeOperations.h index 471b38376..453678611 100644 --- a/gtsam_unstable/linear/bayesTreeOperations.h +++ b/gtsam_unstable/linear/bayesTreeOperations.h @@ -67,13 +67,33 @@ GTSAM_UNSTABLE_EXPORT std::deque findPathCliques(const GaussianBayesTree::sharedClique& initial); /** - * Liquefies a GaussianBayesTree into a GaussianFactorGraph recursively, given either a - * root clique or a full bayes tree. + * Liquefies a BayesTree into a GaussianFactorGraph recursively, given a + * root clique * * @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors */ -GTSAM_UNSTABLE_EXPORT GaussianFactorGraph liquefy(const GaussianBayesTree::sharedClique& root, bool splitConditionals = false); -GTSAM_UNSTABLE_EXPORT GaussianFactorGraph liquefy(const GaussianBayesTree& bayesTree, bool splitConditionals = false); +template +GaussianFactorGraph liquefy(const typename BAYESTREE::sharedClique& root, bool splitConditionals = false) { + GaussianFactorGraph result; + if (root && root->conditional()) { + GaussianConditional::shared_ptr conditional = root->conditional(); + if (!splitConditionals) + result.push_back(conditional->toFactor()); + else + result.push_back(splitFactor(conditional->toFactor())); + } + BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, root->children()) + result.push_back(liquefy(child, splitConditionals)); + return result; +} + +/** + * Liquefies a BayesTree into a GaussianFactorGraph recursively, from a full bayes tree. + */ +template +GaussianFactorGraph liquefy(const BAYESTREE& bayesTree, bool splitConditionals = false) { + return liquefy(bayesTree.root(), splitConditionals); +} } // \namespace gtsam From 9e3bfdc4f8f82d095a4a4aae216bb8b5c7ee291c Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 11 Jun 2013 14:36:54 +0000 Subject: [PATCH 35/47] comments only --- gtsam_unstable/linear/bayesTreeOperations.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam_unstable/linear/bayesTreeOperations.h b/gtsam_unstable/linear/bayesTreeOperations.h index 453678611..3f85dc12c 100644 --- a/gtsam_unstable/linear/bayesTreeOperations.h +++ b/gtsam_unstable/linear/bayesTreeOperations.h @@ -89,6 +89,8 @@ GaussianFactorGraph liquefy(const typename BAYESTREE::sharedClique& root, bool s /** * Liquefies a BayesTree into a GaussianFactorGraph recursively, from a full bayes tree. + * + * @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors */ template GaussianFactorGraph liquefy(const BAYESTREE& bayesTree, bool splitConditionals = false) { From 23b326824a4783fae9e35e5795c931704d46aa66 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 11 Jun 2013 20:26:34 +0000 Subject: [PATCH 36/47] Added actual check to a test --- gtsam/inference/tests/testPermutation.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/inference/tests/testPermutation.cpp b/gtsam/inference/tests/testPermutation.cpp index 98e94b4f7..5aa83422f 100644 --- a/gtsam/inference/tests/testPermutation.cpp +++ b/gtsam/inference/tests/testPermutation.cpp @@ -125,6 +125,7 @@ TEST(Reduction, CreateFromPartialPermutation) { expected.insert(make_pair(6,4)); internal::Reduction actual = internal::Reduction::CreateFromPartialPermutation(selector, p); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ From 219695318830f2bac97e277f64e7b2f6f38ae92c Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 11 Jun 2013 20:26:34 +0000 Subject: [PATCH 37/47] Removed reference to nonexistant test_lib --- tests/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 021a7dd97..3afbf222a 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -34,7 +34,7 @@ if (GTSAM_BUILD_TESTS) # Build grouped tests gtsam_add_grouped_scripts("tests" # Use subdirectory as group label "test*.cpp" check "Test" # Standard for all tests - "${tests_local_libs}" "${gtsam-default};CppUnitLite;test_lib" "${tests_exclude}" # Pass in linking and exclusion lists + "${tests_local_libs}" "${gtsam-default};CppUnitLite" "${tests_exclude}" # Pass in linking and exclusion lists ${is_test}) # Set all as tests endif (GTSAM_BUILD_TESTS) @@ -47,6 +47,6 @@ if (GTSAM_BUILD_TIMING) # Build grouped benchmarks gtsam_add_grouped_scripts("tests" # Use subdirectory as group label "time*.cpp" timing "Timing Benchmark" # Standard for all timing scripts - "${tests_local_libs}" "${gtsam-default};CppUnitLite;test_lib" "${tests_exclude}" # Pass in linking and exclusion lists + "${tests_local_libs}" "${gtsam-default};CppUnitLite" "${tests_exclude}" # Pass in linking and exclusion lists ${is_test}) endif (GTSAM_BUILD_TIMING) From 19f7da62dd1b072f390c2f875524581eaceaba7f Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 12 Jun 2013 19:30:20 +0000 Subject: [PATCH 38/47] Refactored existing serialization functionality, added exposed interface for serialization --- .cproject | 72 ++----- gtsam/base/serialization.h | 92 +++++++++ gtsam/base/serializationTestHelpers.h | 74 +------ gtsam/slam/serialization.cpp | 257 +++++++++++++++++++++++++ gtsam/slam/serialization.h | 40 ++++ gtsam/slam/tests/testSerialization.cpp | 75 ++++++++ tests/testSerializationSLAM.cpp | 25 ++- 7 files changed, 503 insertions(+), 132 deletions(-) create mode 100644 gtsam/base/serialization.h create mode 100644 gtsam/slam/serialization.cpp create mode 100644 gtsam/slam/serialization.h create mode 100644 gtsam/slam/tests/testSerialization.cpp diff --git a/.cproject b/.cproject index 2145af336..46f9a60fb 100644 --- a/.cproject +++ b/.cproject @@ -619,54 +619,6 @@ true true - - make - -j5 - testPlanarSLAM.run - true - true - true - - - make - -j5 - testPose2SLAM.run - true - true - true - - - make - -j5 - testPose3SLAM.run - true - true - true - - - make - -j5 - testSimulated2D.run - true - true - true - - - make - -j5 - testSimulated2DOriented.run - true - true - true - - - make - -j5 - testVisualSLAM.run - true - true - true - make -j5 @@ -675,14 +627,6 @@ true true - - make - -j5 - testSerializationSLAM.run - true - true - true - make -j5 @@ -691,6 +635,14 @@ true true + + make + -j5 + testSerialization.run + true + true + true + make -j5 @@ -1163,6 +1115,14 @@ true true + + make + -j5 + testSerializationSLAM.run + true + true + true + make -j5 diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h new file mode 100644 index 000000000..8089de814 --- /dev/null +++ b/gtsam/base/serialization.h @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file serialization.h + * @brief Convenience functions for serializing data structures via boost.serialization + * @author Alex Cunningham + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#pragma once + +#include +#include + +// includes for standard serialization types +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +// Serialization directly to strings in compressed format +template +std::string serialize(const T& input) { + std::ostringstream out_archive_stream; + boost::archive::text_oarchive out_archive(out_archive_stream); + out_archive << input; + return out_archive_stream.str(); +} + +template +void deserialize(const std::string& serialized, T& output) { + std::istringstream in_archive_stream(serialized); + boost::archive::text_iarchive in_archive(in_archive_stream); + in_archive >> output; +} + +// Serialization to XML format with named structures +template +std::string serializeXML(const T& input, const std::string& name="data") { + std::ostringstream out_archive_stream; + boost::archive::xml_oarchive out_archive(out_archive_stream); + out_archive << boost::serialization::make_nvp(name.c_str(), input); + return out_archive_stream.str(); +} + +template +void deserializeXML(const std::string& serialized, T& output, const std::string& name="data") { + std::istringstream in_archive_stream(serialized); + boost::archive::xml_iarchive in_archive(in_archive_stream); + in_archive >> boost::serialization::make_nvp(name.c_str(), output); +} + +// Serialization to binary format with named structures +template +std::string serializeBinary(const T& input, const std::string& name="data") { + std::ostringstream out_archive_stream; + boost::archive::binary_oarchive out_archive(out_archive_stream); + out_archive << boost::serialization::make_nvp(name.c_str(), input); + return out_archive_stream.str(); +} + +template +void deserializeBinary(const std::string& serialized, T& output, const std::string& name="data") { + std::istringstream in_archive_stream(serialized); + boost::archive::binary_iarchive in_archive(in_archive_stream); + in_archive >> boost::serialization::make_nvp(name.c_str(), output); +} + +} // \namespace gtsam diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 3d5bf4d71..b6593bd9f 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -22,46 +22,14 @@ #include #include -// includes for standard serialization types -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include +#include // whether to print the serialized text to stdout const bool verbose = false; -namespace gtsam { namespace serializationTestHelpers { +namespace gtsam { +namespace serializationTestHelpers { - /* ************************************************************************* */ - // Serialization testing code. - /* ************************************************************************* */ - -template -std::string serialize(const T& input) { - std::ostringstream out_archive_stream; - boost::archive::text_oarchive out_archive(out_archive_stream); - out_archive << input; - return out_archive_stream.str(); -} - -template -void deserialize(const std::string& serialized, T& output) { - std::istringstream in_archive_stream(serialized); - boost::archive::text_iarchive in_archive(in_archive_stream); - in_archive >> output; -} // Templated round-trip serialization template @@ -97,22 +65,6 @@ bool equalsDereferenced(const T& input) { return input->equals(*output); } -/* ************************************************************************* */ -template -std::string serializeXML(const T& input) { - std::ostringstream out_archive_stream; - boost::archive::xml_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp("data", input); - return out_archive_stream.str(); -} - -template -void deserializeXML(const std::string& serialized, T& output) { - std::istringstream in_archive_stream(serialized); - boost::archive::xml_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp("data", output); -} - // Templated round-trip serialization using XML template void roundtripXML(const T& input, T& output) { @@ -148,22 +100,6 @@ bool equalsDereferencedXML(const T& input = T()) { return input->equals(*output); } -/* ************************************************************************* */ -template -std::string serializeBinary(const T& input) { - std::ostringstream out_archive_stream; - boost::archive::binary_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp("data", input); - return out_archive_stream.str(); -} - -template -void deserializeBinary(const std::string& serialized, T& output) { - std::istringstream in_archive_stream(serialized); - boost::archive::binary_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp("data", output); -} - // Templated round-trip serialization using XML template void roundtripBinary(const T& input, T& output) { @@ -199,4 +135,6 @@ bool equalsDereferencedBinary(const T& input = T()) { return input->equals(*output); } -} } +} // \namespace serializationTestHelpers +} // \namespace gtsam + diff --git a/gtsam/slam/serialization.cpp b/gtsam/slam/serialization.cpp new file mode 100644 index 000000000..a57d82add --- /dev/null +++ b/gtsam/slam/serialization.cpp @@ -0,0 +1,257 @@ +/** + * @file serialization.cpp + * + * @date Jun 12, 2013 + * @author Alex Cunningham + */ + +#include +#include + +//#include +#include +#include +#include +//#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +#include +#include +#include +//#include + +using namespace gtsam; + +// Creating as many permutations of factors as possible +typedef PriorFactor PriorFactorLieVector; +typedef PriorFactor PriorFactorLieMatrix; +typedef PriorFactor PriorFactorPoint2; +typedef PriorFactor PriorFactorStereoPoint2; +typedef PriorFactor PriorFactorPoint3; +typedef PriorFactor PriorFactorRot2; +typedef PriorFactor PriorFactorRot3; +typedef PriorFactor PriorFactorPose2; +typedef PriorFactor PriorFactorPose3; +typedef PriorFactor PriorFactorCal3_S2; +typedef PriorFactor PriorFactorCal3DS2; +typedef PriorFactor PriorFactorCalibratedCamera; +typedef PriorFactor PriorFactorSimpleCamera; +typedef PriorFactor PriorFactorStereoCamera; + +typedef BetweenFactor BetweenFactorLieVector; +typedef BetweenFactor BetweenFactorLieMatrix; +typedef BetweenFactor BetweenFactorPoint2; +typedef BetweenFactor BetweenFactorPoint3; +typedef BetweenFactor BetweenFactorRot2; +typedef BetweenFactor BetweenFactorRot3; +typedef BetweenFactor BetweenFactorPose2; +typedef BetweenFactor BetweenFactorPose3; + +typedef NonlinearEquality NonlinearEqualityLieVector; +typedef NonlinearEquality NonlinearEqualityLieMatrix; +typedef NonlinearEquality NonlinearEqualityPoint2; +typedef NonlinearEquality NonlinearEqualityStereoPoint2; +typedef NonlinearEquality NonlinearEqualityPoint3; +typedef NonlinearEquality NonlinearEqualityRot2; +typedef NonlinearEquality NonlinearEqualityRot3; +typedef NonlinearEquality NonlinearEqualityPose2; +typedef NonlinearEquality NonlinearEqualityPose3; +typedef NonlinearEquality NonlinearEqualityCal3_S2; +typedef NonlinearEquality NonlinearEqualityCal3DS2; +typedef NonlinearEquality NonlinearEqualityCalibratedCamera; +typedef NonlinearEquality NonlinearEqualitySimpleCamera; +typedef NonlinearEquality NonlinearEqualityStereoCamera; + +typedef RangeFactor RangeFactorPosePoint2; +typedef RangeFactor RangeFactorPosePoint3; +typedef RangeFactor RangeFactorPose2; +typedef RangeFactor RangeFactorPose3; +typedef RangeFactor RangeFactorCalibratedCameraPoint; +typedef RangeFactor RangeFactorSimpleCameraPoint; +typedef RangeFactor RangeFactorCalibratedCamera; +typedef RangeFactor RangeFactorSimpleCamera; + +typedef BearingFactor BearingFactor2D; +typedef BearingFactor BearingFactor3D; + +typedef BearingRangeFactor BearingRangeFactor2D; +typedef BearingRangeFactor BearingRangeFactor3D; + +typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; +typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; + +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +//typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; + +typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; + +typedef gtsam::GenericStereoFactor GenericStereoFactor3D; + + +/* Create GUIDs for Noisemodels */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); + +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); + +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +/* Create GUIDs for geometry */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT(gtsam::LieVector); +BOOST_CLASS_EXPORT(gtsam::LieMatrix); +BOOST_CLASS_EXPORT(gtsam::Point2); +BOOST_CLASS_EXPORT(gtsam::StereoPoint2); +BOOST_CLASS_EXPORT(gtsam::Point3); +BOOST_CLASS_EXPORT(gtsam::Rot2); +BOOST_CLASS_EXPORT(gtsam::Rot3); +BOOST_CLASS_EXPORT(gtsam::Pose2); +BOOST_CLASS_EXPORT(gtsam::Pose3); +BOOST_CLASS_EXPORT(gtsam::Cal3_S2); +BOOST_CLASS_EXPORT(gtsam::Cal3DS2); +BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo); +BOOST_CLASS_EXPORT(gtsam::CalibratedCamera); +BOOST_CLASS_EXPORT(gtsam::SimpleCamera); +BOOST_CLASS_EXPORT(gtsam::StereoCamera); + +/* Create GUIDs for factors */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); + +BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector"); +BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3"); +BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3"); +BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); +BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); +BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera"); +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera"); + +BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorLieMatrix, "gtsam::BetweenFactorLieMatrix"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3"); + +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieVector, "gtsam::NonlinearEqualityLieVector"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieMatrix, "gtsam::NonlinearEqualityLieMatrix"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); + +BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint2, "gtsam::RangeFactorPosePoint2"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPosePoint3, "gtsam::RangeFactorPosePoint3"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); +BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); +BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint"); +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); +BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); + +BOOST_CLASS_EXPORT_GUID(BearingFactor2D, "gtsam::BearingFactor2D"); + +BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); + +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2"); +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); + +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); +//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); + +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); + +BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); + +/* ************************************************************************* */ +// Actual implementations of functions +/* ************************************************************************* */ +std::string gtsam::serializeGraph(const NonlinearFactorGraph& graph) { + return serialize(graph); +} + +/* ************************************************************************* */ +NonlinearFactorGraph gtsam::deserializeGraph(const std::string& serialized_graph) { + NonlinearFactorGraph result; + deserialize(serialized_graph, result); + return result; +} + +/* ************************************************************************* */ +std::string gtsam::serializeGraphXML(const NonlinearFactorGraph& graph, const std::string& name) { + return serializeXML(graph, name); +} + +/* ************************************************************************* */ +NonlinearFactorGraph gtsam::deserializeGraphXML(const std::string& serialized_graph, + const std::string& name) { + NonlinearFactorGraph result; + deserializeXML(serialized_graph, result, name); + return result; +} + +/* ************************************************************************* */ +std::string gtsam::serializeValues(const Values& values) { + return serialize(values); +} + +/* ************************************************************************* */ +Values gtsam::deserializeValues(const std::string& serialized_values) { + Values result; + deserialize(serialized_values, result); + return result; +} + +/* ************************************************************************* */ +std::string gtsam::serializeValuesXML(const Values& values, const std::string& name) { + return serializeXML(values, name); +} + +/* ************************************************************************* */ +Values gtsam::deserializeValuesXML(const std::string& serialized_values, + const std::string& name) { + Values result; + deserializeXML(serialized_values, result, name); + return result; +} + +/* ************************************************************************* */ + + diff --git a/gtsam/slam/serialization.h b/gtsam/slam/serialization.h new file mode 100644 index 000000000..ceb237686 --- /dev/null +++ b/gtsam/slam/serialization.h @@ -0,0 +1,40 @@ +/** + * @file serialization.h + * + * @brief Global functions for performing serialization, designed for use with matlab + * + * @date Jun 12, 2013 + * @author Alex Cunningham + */ + +#pragma once + +#include + +namespace gtsam { + +// Serialize/Deserialize a NonlinearFactorGraph +std::string serializeGraph(const NonlinearFactorGraph& graph); + +NonlinearFactorGraph deserializeGraph(const std::string& serialized_graph); + +std::string serializeGraphXML(const NonlinearFactorGraph& graph, + const std::string& name = "graph"); + +NonlinearFactorGraph deserializeGraphXML(const std::string& serialized_graph, + const std::string& name = "graph"); + + +// Serialize/Deserialize a Values +std::string serializeValues(const Values& values); + +Values deserializeValues(const std::string& serialized_values); + +std::string serializeValuesXML(const Values& values, const std::string& name = "values"); + +Values deserializeValuesXML(const std::string& serialized_values, + const std::string& name = "values"); + +} // \namespace gtsam + + diff --git a/gtsam/slam/tests/testSerialization.cpp b/gtsam/slam/tests/testSerialization.cpp new file mode 100644 index 000000000..64d5504da --- /dev/null +++ b/gtsam/slam/tests/testSerialization.cpp @@ -0,0 +1,75 @@ +/** + * @file testSerialization.cpp + * + * @brief Tests for serialization global functions using boost.serialization + * + * @date Jun 12, 2013 + * @author Alex Cunningham + */ + +#include + +#include + +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +Values exampleValues() { + Values result; + result.insert(234, gtsam::Rot2::fromAngle(0.1)); + result.insert(123, gtsam::Point2(1.0, 2.0)); + result.insert(254, gtsam::Pose2(1.0, 2.0, 0.3)); + result.insert(678, gtsam::Rot3::Rx(0.1)); + result.insert(498, gtsam::Point3(1.0, 2.0, 3.0)); + result.insert(345, gtsam::Pose3(Rot3::Rx(0.1), Point3(1.0, 2.0, 3.0))); + return result; +} + +NonlinearFactorGraph exampleGraph() { + NonlinearFactorGraph graph; + graph.add(PriorFactor(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); + graph.add(BetweenFactor(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); + return graph; +} + +/* ************************************************************************* */ +TEST( testSerialization, text_graph_serialization ) { + NonlinearFactorGraph graph = exampleGraph(); + string serialized = serializeGraph(graph); + NonlinearFactorGraph actGraph = deserializeGraph(serialized); + EXPECT(assert_equal(graph, actGraph)); +} + +/* ************************************************************************* */ +TEST( testSerialization, xml_graph_serialization ) { + NonlinearFactorGraph graph = exampleGraph(); + string serialized = serializeGraphXML(graph, "graph1"); + NonlinearFactorGraph actGraph = deserializeGraphXML(serialized, "graph1"); + EXPECT(assert_equal(graph, actGraph)); +} + +/* ************************************************************************* */ +TEST( testSerialization, text_values_serialization ) { + Values values = exampleValues(); + string serialized = serializeValues(values); + Values actValues = deserializeValues(serialized); + EXPECT(assert_equal(values, actValues)); +} + +/* ************************************************************************* */ +TEST( testSerialization, xml_values_serialization ) { + Values values = exampleValues(); + string serialized = serializeValuesXML(values, "values1"); + Values actValues = deserializeValuesXML(serialized, "values1"); + EXPECT(assert_equal(values, actValues, 1e-5)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index f9229eac6..57a1e5cb3 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -133,6 +133,13 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal" BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust"); + +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); @@ -155,7 +162,6 @@ BOOST_CLASS_EXPORT(gtsam::CalibratedCamera); BOOST_CLASS_EXPORT(gtsam::SimpleCamera); BOOST_CLASS_EXPORT(gtsam::StereoCamera); - /* Create GUIDs for factors */ /* ************************************************************************* */ BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); @@ -225,7 +231,7 @@ BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); /* ************************************************************************* */ -TEST (Serialization, smallExample_linear) { +TEST (testSerializationSLAM, smallExample_linear) { using namespace example; Ordering ordering; ordering += X(1),X(2),L(1); @@ -245,7 +251,7 @@ TEST (Serialization, smallExample_linear) { } /* ************************************************************************* */ -TEST (Serialization, gaussianISAM) { +TEST (testSerializationSLAM, gaussianISAM) { using namespace example; Ordering ordering; GaussianFactorGraph smoother; @@ -265,7 +271,7 @@ BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement") /* ************************************************************************* */ -TEST (Serialization, smallExample_nonlinear) { +TEST (testSerializationSLAM, smallExample_nonlinear) { using namespace example; NonlinearFactorGraph nfg = createNonlinearFactorGraph(); Values c1 = createValues(); @@ -279,7 +285,7 @@ TEST (Serialization, smallExample_nonlinear) { } /* ************************************************************************* */ -TEST (Serialization, factors) { +TEST (testSerializationSLAM, factors) { LieVector lieVector(4, 1.0, 2.0, 3.0, 4.0); LieMatrix lieMatrix(2, 3, 1.0, 2.0, 3.0, 4.0, 5.0 ,6.0); @@ -331,7 +337,13 @@ TEST (Serialization, factors) { SharedNoiseModel model9 = noiseModel::Isotropic::Sigma(9, 0.3); SharedNoiseModel model11 = noiseModel::Isotropic::Sigma(11, 0.3); + SharedNoiseModel robust1 = noiseModel::Robust::Create( + noiseModel::mEstimator::Huber::Create(10.0, noiseModel::mEstimator::Huber::Scalar), + noiseModel::Unit::Create(2)); + EXPECT(equalsDereferenced(robust1)); + EXPECT(equalsDereferencedXML(robust1)); + EXPECT(equalsDereferencedBinary(robust1)); PriorFactorLieVector priorFactorLieVector(a01, lieVector, model4); PriorFactorLieMatrix priorFactorLieMatrix(a02, lieMatrix, model6); @@ -660,11 +672,8 @@ TEST (Serialization, factors) { EXPECT(equalsBinary(generalSFMFactor2Cal3_S2)); EXPECT(equalsBinary(genericStereoFactor3D)); - } - - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 666e30a59b136521f1168971a053b55c58fdb536 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 12 Jun 2013 19:30:21 +0000 Subject: [PATCH 39/47] Changed serialization interface to return shared versions of graph/values to avoid copies in matlab --- gtsam/slam/serialization.cpp | 24 ++++++++++++------------ gtsam/slam/serialization.h | 8 ++++---- gtsam/slam/tests/testSerialization.cpp | 8 ++++---- 3 files changed, 20 insertions(+), 20 deletions(-) diff --git a/gtsam/slam/serialization.cpp b/gtsam/slam/serialization.cpp index a57d82add..d8a1cce06 100644 --- a/gtsam/slam/serialization.cpp +++ b/gtsam/slam/serialization.cpp @@ -208,9 +208,9 @@ std::string gtsam::serializeGraph(const NonlinearFactorGraph& graph) { } /* ************************************************************************* */ -NonlinearFactorGraph gtsam::deserializeGraph(const std::string& serialized_graph) { - NonlinearFactorGraph result; - deserialize(serialized_graph, result); +NonlinearFactorGraph::shared_ptr gtsam::deserializeGraph(const std::string& serialized_graph) { + NonlinearFactorGraph::shared_ptr result(new NonlinearFactorGraph()); + deserialize(serialized_graph, *result); return result; } @@ -220,10 +220,10 @@ std::string gtsam::serializeGraphXML(const NonlinearFactorGraph& graph, const st } /* ************************************************************************* */ -NonlinearFactorGraph gtsam::deserializeGraphXML(const std::string& serialized_graph, +NonlinearFactorGraph::shared_ptr gtsam::deserializeGraphXML(const std::string& serialized_graph, const std::string& name) { - NonlinearFactorGraph result; - deserializeXML(serialized_graph, result, name); + NonlinearFactorGraph::shared_ptr result(new NonlinearFactorGraph()); + deserializeXML(serialized_graph, *result, name); return result; } @@ -233,9 +233,9 @@ std::string gtsam::serializeValues(const Values& values) { } /* ************************************************************************* */ -Values gtsam::deserializeValues(const std::string& serialized_values) { - Values result; - deserialize(serialized_values, result); +Values::shared_ptr gtsam::deserializeValues(const std::string& serialized_values) { + Values::shared_ptr result(new Values()); + deserialize(serialized_values, *result); return result; } @@ -245,10 +245,10 @@ std::string gtsam::serializeValuesXML(const Values& values, const std::string& n } /* ************************************************************************* */ -Values gtsam::deserializeValuesXML(const std::string& serialized_values, +Values::shared_ptr gtsam::deserializeValuesXML(const std::string& serialized_values, const std::string& name) { - Values result; - deserializeXML(serialized_values, result, name); + Values::shared_ptr result(new Values()); + deserializeXML(serialized_values, *result, name); return result; } diff --git a/gtsam/slam/serialization.h b/gtsam/slam/serialization.h index ceb237686..0dde72a8c 100644 --- a/gtsam/slam/serialization.h +++ b/gtsam/slam/serialization.h @@ -16,23 +16,23 @@ namespace gtsam { // Serialize/Deserialize a NonlinearFactorGraph std::string serializeGraph(const NonlinearFactorGraph& graph); -NonlinearFactorGraph deserializeGraph(const std::string& serialized_graph); +NonlinearFactorGraph::shared_ptr deserializeGraph(const std::string& serialized_graph); std::string serializeGraphXML(const NonlinearFactorGraph& graph, const std::string& name = "graph"); -NonlinearFactorGraph deserializeGraphXML(const std::string& serialized_graph, +NonlinearFactorGraph::shared_ptr deserializeGraphXML(const std::string& serialized_graph, const std::string& name = "graph"); // Serialize/Deserialize a Values std::string serializeValues(const Values& values); -Values deserializeValues(const std::string& serialized_values); +Values::shared_ptr deserializeValues(const std::string& serialized_values); std::string serializeValuesXML(const Values& values, const std::string& name = "values"); -Values deserializeValuesXML(const std::string& serialized_values, +Values::shared_ptr deserializeValuesXML(const std::string& serialized_values, const std::string& name = "values"); } // \namespace gtsam diff --git a/gtsam/slam/tests/testSerialization.cpp b/gtsam/slam/tests/testSerialization.cpp index 64d5504da..91b938ad0 100644 --- a/gtsam/slam/tests/testSerialization.cpp +++ b/gtsam/slam/tests/testSerialization.cpp @@ -42,7 +42,7 @@ NonlinearFactorGraph exampleGraph() { TEST( testSerialization, text_graph_serialization ) { NonlinearFactorGraph graph = exampleGraph(); string serialized = serializeGraph(graph); - NonlinearFactorGraph actGraph = deserializeGraph(serialized); + NonlinearFactorGraph actGraph = *deserializeGraph(serialized); EXPECT(assert_equal(graph, actGraph)); } @@ -50,7 +50,7 @@ TEST( testSerialization, text_graph_serialization ) { TEST( testSerialization, xml_graph_serialization ) { NonlinearFactorGraph graph = exampleGraph(); string serialized = serializeGraphXML(graph, "graph1"); - NonlinearFactorGraph actGraph = deserializeGraphXML(serialized, "graph1"); + NonlinearFactorGraph actGraph = *deserializeGraphXML(serialized, "graph1"); EXPECT(assert_equal(graph, actGraph)); } @@ -58,7 +58,7 @@ TEST( testSerialization, xml_graph_serialization ) { TEST( testSerialization, text_values_serialization ) { Values values = exampleValues(); string serialized = serializeValues(values); - Values actValues = deserializeValues(serialized); + Values actValues = *deserializeValues(serialized); EXPECT(assert_equal(values, actValues)); } @@ -66,7 +66,7 @@ TEST( testSerialization, text_values_serialization ) { TEST( testSerialization, xml_values_serialization ) { Values values = exampleValues(); string serialized = serializeValuesXML(values, "values1"); - Values actValues = deserializeValuesXML(serialized, "values1"); + Values actValues = *deserializeValuesXML(serialized, "values1"); EXPECT(assert_equal(values, actValues, 1e-5)); } From e69af84c36970fbbeb3de209e3bfe06cb034de69 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 12 Jun 2013 19:30:22 +0000 Subject: [PATCH 40/47] Added wrapping for graph/values serialization with tests in Matlab. Values serializes correctly, but graphs do not in either case. --- gtsam.h | 27 ++++++++++++++++++++++ matlab/gtsam_tests/testPlanarSLAMExample.m | 8 +++++++ matlab/gtsam_tests/testPose2SLAMExample.m | 9 ++++++++ 3 files changed, 44 insertions(+) diff --git a/gtsam.h b/gtsam.h index 20ea0bf8a..64270b133 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2102,6 +2102,33 @@ pair load2D(string filename, pair load2D_robust(string filename, gtsam::noiseModel::Base* model); +//************************************************************************* +// Serialization +//************************************************************************* +#include + +// Serialize/Deserialize a NonlinearFactorGraph +string serializeGraph(const gtsam::NonlinearFactorGraph& graph); + +gtsam::NonlinearFactorGraph* deserializeGraph(string serialized_graph); + +string serializeGraphXML(const gtsam::NonlinearFactorGraph& graph); +string serializeGraphXML(const gtsam::NonlinearFactorGraph& graph, string name); + +gtsam::NonlinearFactorGraph* deserializeGraphXML(string serialized_graph); +gtsam::NonlinearFactorGraph* deserializeGraphXML(string serialized_graph, string name); + +// Serialize/Deserialize a Values +string serializeValues(const gtsam::Values& values); + +gtsam::Values* deserializeValues(string serialized_values); + +string serializeValuesXML(const gtsam::Values& values); +string serializeValuesXML(const gtsam::Values& values, string name); + +gtsam::Values* deserializeValuesXML(string serialized_values); +gtsam::Values* deserializeValuesXML(string serialized_values, string name); + //************************************************************************* // Utilities //************************************************************************* diff --git a/matlab/gtsam_tests/testPlanarSLAMExample.m b/matlab/gtsam_tests/testPlanarSLAMExample.m index 2a736a710..584b86345 100644 --- a/matlab/gtsam_tests/testPlanarSLAMExample.m +++ b/matlab/gtsam_tests/testPlanarSLAMExample.m @@ -68,4 +68,12 @@ point_1 = result.at(symbol('l',1)); marginals.marginalCovariance(symbol('l',1)); CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4)); +%% Check that serialization works properly +serialized_values = serializeValues(initialEstimate); +deserializedValues = deserializeValues(serialized_values); +CHECK('initialEstimate.equals(deserializedValues)',initialEstimate.equals(deserializedValues,1e-9)); +% FAILS: unregistered class? +% serialized_graph = serializeGraph(graph); +% deserializedGraph = deserializeGraph(serialized_graph); +% CHECK('graph.equals(deserializedGraph)',graph.equals(deserializedGraph,1e-9)); \ No newline at end of file diff --git a/matlab/gtsam_tests/testPose2SLAMExample.m b/matlab/gtsam_tests/testPose2SLAMExample.m index 127bcd41c..2ac354955 100644 --- a/matlab/gtsam_tests/testPose2SLAMExample.m +++ b/matlab/gtsam_tests/testPose2SLAMExample.m @@ -61,3 +61,12 @@ pose_1 = result.at(1); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); +%% Check that serialization works properly +serialized_values = serializeValues(initialEstimate); +deserializedValues = deserializeValues(serialized_values); +CHECK('initialEstimate.equals(deserializedValues)',initialEstimate.equals(deserializedValues,1e-9)); + +% FAILS: unregistered class? +% serialized_graph = serializeGraph(graph); +% deserializedGraph = deserializeGraph(serialized_graph); +% CHECK('graph.equals(deserializedGraph)',graph.equals(deserializedGraph,1e-9)); From 527ea5e5114220a663c8a14192a49ade6278d762 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 12 Jun 2013 20:01:59 +0000 Subject: [PATCH 41/47] Moved serialization tests over to a single test scenario - factors don't appear to work at the moment --- .../testGraphValuesSerialization.m | 56 +++++++++++++++++++ matlab/gtsam_tests/testPlanarSLAMExample.m | 10 ---- matlab/gtsam_tests/testPose2SLAMExample.m | 10 ---- matlab/gtsam_tests/test_gtsam.m | 3 + 4 files changed, 59 insertions(+), 20 deletions(-) create mode 100644 matlab/gtsam_tests/testGraphValuesSerialization.m diff --git a/matlab/gtsam_tests/testGraphValuesSerialization.m b/matlab/gtsam_tests/testGraphValuesSerialization.m new file mode 100644 index 000000000..1d4066c82 --- /dev/null +++ b/matlab/gtsam_tests/testGraphValuesSerialization.m @@ -0,0 +1,56 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% GTSAM Copyright 2010, Georgia Tech Research Corporation, +% Atlanta, Georgia 30332-0415 +% All Rights Reserved +% Authors: Frank Dellaert, et al. (see THANKS for the full author list) +% +% See LICENSE for the license information +% +% @brief Simple robotics example using the pre-built planar SLAM domain +% @author Alex Cunningham +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +import gtsam.* + +%% Create keys for variables +i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3); +j1 = symbol('l',1); j2 = symbol('l',2); + +%% Create graph and factors +graph = NonlinearFactorGraph; + +% Prior factor - FAIL: unregistered class +priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin +priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); +graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph + +% Between Factors - FAIL: unregistered class +odometry = Pose2(2.0, 0.0, 0.0); +odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); +graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise)); +graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise)); + +% BearingRange Factors - FAIL: unregistered class +degrees = pi/180; +brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]); +graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise)); +graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise)); +graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise)); + +%% Create Values +values = Values; +values.insert(i1, Pose2(0.5, 0.0, 0.2)); +values.insert(i2, Pose2(2.3, 0.1,-0.2)); +values.insert(i3, Pose2(4.1, 0.1, 0.1)); +values.insert(j1, Point2(1.8, 2.1)); +values.insert(j2, Point2(4.1, 1.8)); + +%% Check that serialization works properly +serialized_values = serializeValues(values); % returns a string +deserializedValues = deserializeValues(serialized_values); % returns a new values +CHECK('values.equals(deserializedValues)',values.equals(deserializedValues,1e-9)); + +serialized_graph = serializeGraph(graph); % returns a string +deserializedGraph = deserializeGraph(serialized_graph); % returns a new graph +CHECK('graph.equals(deserializedGraph)',graph.equals(deserializedGraph,1e-9)); \ No newline at end of file diff --git a/matlab/gtsam_tests/testPlanarSLAMExample.m b/matlab/gtsam_tests/testPlanarSLAMExample.m index 584b86345..7694a1a0b 100644 --- a/matlab/gtsam_tests/testPlanarSLAMExample.m +++ b/matlab/gtsam_tests/testPlanarSLAMExample.m @@ -67,13 +67,3 @@ CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); point_1 = result.at(symbol('l',1)); marginals.marginalCovariance(symbol('l',1)); CHECK('point_1.equals(Point2(2,2),1e-4)',point_1.equals(Point2(2,2),1e-4)); - -%% Check that serialization works properly -serialized_values = serializeValues(initialEstimate); -deserializedValues = deserializeValues(serialized_values); -CHECK('initialEstimate.equals(deserializedValues)',initialEstimate.equals(deserializedValues,1e-9)); - -% FAILS: unregistered class? -% serialized_graph = serializeGraph(graph); -% deserializedGraph = deserializeGraph(serialized_graph); -% CHECK('graph.equals(deserializedGraph)',graph.equals(deserializedGraph,1e-9)); \ No newline at end of file diff --git a/matlab/gtsam_tests/testPose2SLAMExample.m b/matlab/gtsam_tests/testPose2SLAMExample.m index 2ac354955..8c70c27e7 100644 --- a/matlab/gtsam_tests/testPose2SLAMExample.m +++ b/matlab/gtsam_tests/testPose2SLAMExample.m @@ -60,13 +60,3 @@ P = marginals.marginalCovariance(1); pose_1 = result.at(1); CHECK('pose_1.equals(Pose2,1e-4)',pose_1.equals(Pose2,1e-4)); - -%% Check that serialization works properly -serialized_values = serializeValues(initialEstimate); -deserializedValues = deserializeValues(serialized_values); -CHECK('initialEstimate.equals(deserializedValues)',initialEstimate.equals(deserializedValues,1e-9)); - -% FAILS: unregistered class? -% serialized_graph = serializeGraph(graph); -% deserializedGraph = deserializeGraph(serialized_graph); -% CHECK('graph.equals(deserializedGraph)',graph.equals(deserializedGraph,1e-9)); diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 113e594fd..1f8125b81 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -30,5 +30,8 @@ testStereoVOExample display 'Starting: testVisualISAMExample' testVisualISAMExample +display 'Starting: testGraphValuesSerialization' +testVisualISAMExample + % end of tests display 'Tests complete!' From e1fc90ad14f319d9ce91bafafb5c77c3f1645aff Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 13 Jun 2013 14:29:31 +0000 Subject: [PATCH 42/47] Added direct saving to/from files for graph/values serialization --- gtsam/base/serialization.h | 67 ++++++++++++++++++++++++++ gtsam/slam/serialization.cpp | 56 +++++++++++++++++++++ gtsam/slam/serialization.h | 22 +++++++++ gtsam/slam/tests/testSerialization.cpp | 47 ++++++++++++++++++ 4 files changed, 192 insertions(+) diff --git a/gtsam/base/serialization.h b/gtsam/base/serialization.h index 8089de814..a41182f47 100644 --- a/gtsam/base/serialization.h +++ b/gtsam/base/serialization.h @@ -20,6 +20,7 @@ #pragma once #include +#include #include // includes for standard serialization types @@ -57,6 +58,28 @@ void deserialize(const std::string& serialized, T& output) { in_archive >> output; } +template +bool serializeToFile(const T& input, const std::string& filename) { + std::ofstream out_archive_stream(filename.c_str()); + if (!out_archive_stream.is_open()) + return false; + boost::archive::text_oarchive out_archive(out_archive_stream); + out_archive << input; + out_archive_stream.close(); + return true; +} + +template +bool deserializeFromFile(const std::string& filename, T& output) { + std::ifstream in_archive_stream(filename.c_str()); + if (!in_archive_stream.is_open()) + return false; + boost::archive::text_iarchive in_archive(in_archive_stream); + in_archive >> output; + in_archive_stream.close(); + return true; +} + // Serialization to XML format with named structures template std::string serializeXML(const T& input, const std::string& name="data") { @@ -73,6 +96,28 @@ void deserializeXML(const std::string& serialized, T& output, const std::string& in_archive >> boost::serialization::make_nvp(name.c_str(), output); } +template +bool serializeToXMLFile(const T& input, const std::string& filename, const std::string& name="data") { + std::ofstream out_archive_stream(filename.c_str()); + if (!out_archive_stream.is_open()) + return false; + boost::archive::xml_oarchive out_archive(out_archive_stream); + out_archive << boost::serialization::make_nvp(name.c_str(), input);; + out_archive_stream.close(); + return true; +} + +template +bool deserializeFromXMLFile(const std::string& filename, T& output, const std::string& name="data") { + std::ifstream in_archive_stream(filename.c_str()); + if (!in_archive_stream.is_open()) + return false; + boost::archive::xml_iarchive in_archive(in_archive_stream); + in_archive >> boost::serialization::make_nvp(name.c_str(), output); + in_archive_stream.close(); + return true; +} + // Serialization to binary format with named structures template std::string serializeBinary(const T& input, const std::string& name="data") { @@ -89,4 +134,26 @@ void deserializeBinary(const std::string& serialized, T& output, const std::stri in_archive >> boost::serialization::make_nvp(name.c_str(), output); } +template +bool deserializeToBinaryFile(const T& input, const std::string& filename, const std::string& name="data") { + std::ofstream out_archive_stream(filename.c_str()); + if (!out_archive_stream.is_open()) + return false; + boost::archive::binary_oarchive out_archive(out_archive_stream); + out_archive << boost::serialization::make_nvp(name.c_str(), input);; + out_archive_stream.close(); + return true; +} + +template +bool deserializeFromBinaryFile(const std::string& filename, T& output, const std::string& name="data") { + std::ifstream in_archive_stream(filename.c_str()); + if (!in_archive_stream.is_open()) + return false; + boost::archive::binary_iarchive in_archive(in_archive_stream); + in_archive >> boost::serialization::make_nvp(name.c_str(), output); + in_archive_stream.close(); + return true; +} + } // \namespace gtsam diff --git a/gtsam/slam/serialization.cpp b/gtsam/slam/serialization.cpp index d8a1cce06..e601a5e81 100644 --- a/gtsam/slam/serialization.cpp +++ b/gtsam/slam/serialization.cpp @@ -253,5 +253,61 @@ Values::shared_ptr gtsam::deserializeValuesXML(const std::string& serialized_val } /* ************************************************************************* */ +bool gtsam::serializeGraphToFile(const NonlinearFactorGraph& graph, const std::string& fname) { + return serializeToFile(graph, fname); +} + +/* ************************************************************************* */ +bool gtsam::serializeGraphToXMLFile(const NonlinearFactorGraph& graph, + const std::string& fname, const std::string& name) { + return serializeToXMLFile(graph, fname, name); +} + +/* ************************************************************************* */ +bool gtsam::serializeValuesToFile(const Values& values, const std::string& fname) { + return serializeToFile(values, fname); +} + +/* ************************************************************************* */ +bool gtsam::serializeValuesToXMLFile(const Values& values, + const std::string& fname, const std::string& name) { + return serializeToXMLFile(values, fname, name); +} + +/* ************************************************************************* */ +NonlinearFactorGraph::shared_ptr gtsam::deserializeGraphToFile(const std::string& fname) { + NonlinearFactorGraph::shared_ptr result(new NonlinearFactorGraph()); + if (!deserializeFromFile(fname, *result)) + throw std::invalid_argument("Failed to open file" + fname); + return result; +} + +/* ************************************************************************* */ +NonlinearFactorGraph::shared_ptr gtsam::deserializeGraphToXMLFile(const std::string& fname, + const std::string& name) { + NonlinearFactorGraph::shared_ptr result(new NonlinearFactorGraph()); + if (!deserializeFromXMLFile(fname, *result, name)) + throw std::invalid_argument("Failed to open file" + fname); + return result; +} + +/* ************************************************************************* */ +Values::shared_ptr gtsam::deserializeValuesToFile(const std::string& fname) { + Values::shared_ptr result(new Values()); + if (!deserializeFromFile(fname, *result)) + throw std::invalid_argument("Failed to open file" + fname); + return result; +} + +/* ************************************************************************* */ +Values::shared_ptr gtsam::deserializeValuesToXMLFile(const std::string& fname, + const std::string& name) { + Values::shared_ptr result(new Values()); + if (!deserializeFromXMLFile(fname, *result, name)) + throw std::invalid_argument("Failed to open file" + fname); + return result; +} + +/* ************************************************************************* */ diff --git a/gtsam/slam/serialization.h b/gtsam/slam/serialization.h index 0dde72a8c..5d75f160c 100644 --- a/gtsam/slam/serialization.h +++ b/gtsam/slam/serialization.h @@ -35,6 +35,28 @@ std::string serializeValuesXML(const Values& values, const std::string& name = " Values::shared_ptr deserializeValuesXML(const std::string& serialized_values, const std::string& name = "values"); +// Serialize to/from files +// serialize functions return true if successful +// Filename arguments include path + +// Serialize +bool serializeGraphToFile(const NonlinearFactorGraph& graph, const std::string& fname); +bool serializeGraphToXMLFile(const NonlinearFactorGraph& graph, + const std::string& fname, const std::string& name = "graph"); + +bool serializeValuesToFile(const Values& values, const std::string& fname); +bool serializeValuesToXMLFile(const Values& values, + const std::string& fname, const std::string& name = "values"); + +// Deserialize +NonlinearFactorGraph::shared_ptr deserializeGraphToFile(const std::string& fname); +NonlinearFactorGraph::shared_ptr deserializeGraphToXMLFile(const std::string& fname, + const std::string& name = "graph"); + +Values::shared_ptr deserializeValuesToFile(const std::string& fname); +Values::shared_ptr deserializeValuesToXMLFile(const std::string& fname, + const std::string& name = "values"); + } // \namespace gtsam diff --git a/gtsam/slam/tests/testSerialization.cpp b/gtsam/slam/tests/testSerialization.cpp index 91b938ad0..aad89d4a0 100644 --- a/gtsam/slam/tests/testSerialization.cpp +++ b/gtsam/slam/tests/testSerialization.cpp @@ -16,9 +16,23 @@ #include #include +#include + +#include +#include +#include +#include +#include using namespace std; using namespace gtsam; +using namespace boost::assign; +namespace fs = boost::filesystem; +#ifdef TOPSRCDIR +static string topdir = TOPSRCDIR; +#else +static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not defined, we error +#endif Values exampleValues() { Values result; @@ -35,6 +49,7 @@ NonlinearFactorGraph exampleGraph() { NonlinearFactorGraph graph; graph.add(PriorFactor(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); graph.add(BetweenFactor(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(ones(3)))); + graph.add(BearingRangeFactor(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(ones(2)))); return graph; } @@ -70,6 +85,38 @@ TEST( testSerialization, xml_values_serialization ) { EXPECT(assert_equal(values, actValues, 1e-5)); } +/* ************************************************************************* */ +TEST( testSerialization, serialization_file ) { + // Create files in folder in build folder + fs::remove_all("actual"); + fs::create_directory("actual"); + string path = "actual/"; + + NonlinearFactorGraph graph = exampleGraph(); + Values values = exampleValues(); + + // Serialize objects using each configuration + EXPECT(serializeGraphToFile(graph, path + "graph.dat")); + EXPECT(serializeGraphToXMLFile(graph, path + "graph.xml", "graph1")); + + EXPECT(serializeValuesToFile(values, path + "values.dat")); + EXPECT(serializeValuesToXMLFile(values, path + "values.xml", "values1")); + + // Deserialize + NonlinearFactorGraph actGraph = *deserializeGraphToFile(path + "graph.dat"); + NonlinearFactorGraph actGraphXML = *deserializeGraphToXMLFile(path + "graph.xml", "graph1"); + + Values actValues = *deserializeValuesToFile(path + "values.dat"); + Values actValuesXML = *deserializeValuesToXMLFile(path + "values.xml", "values1"); + + // Verify + EXPECT(assert_equal(graph, actGraph)); + EXPECT(assert_equal(graph, actGraphXML)); + + EXPECT(assert_equal(values, actValues)); + EXPECT(assert_equal(values, actValuesXML)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From cabcb3efc8d8b3ed286b469fc8e819e0384b6ae2 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 13 Jun 2013 14:29:32 +0000 Subject: [PATCH 43/47] Added wrapping for serialization to/from file functions --- gtsam.h | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/gtsam.h b/gtsam.h index 64270b133..40806c3a2 100644 --- a/gtsam.h +++ b/gtsam.h @@ -62,7 +62,7 @@ * of using the copy constructor (which is used for non-virtual objects). * - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree * virtual boost::shared_ptr clone() const; - * Templates + * Class Templates * - Basic templates are supported either with an explicit list of types to instantiate, * e.g. template class Class1 { ... }; * or with typedefs, e.g. @@ -81,7 +81,12 @@ /** * Status: * - TODO: default values for arguments + * - WORKAROUND: make multiple versions of the same function for different configurations of default arguments * - TODO: Handle gtsam::Rot3M conversions to quaternions + * - TODO: Parse return of const ref arguments + * - TODO: Parse std::string variants and convert directly to special string + * - TODO: Add enum support + * - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load */ namespace std { @@ -98,7 +103,7 @@ namespace std { bool empty() const; void reserve(size_t n); - //Element acces + //Element access T* at(size_t n); T* front(); T* back(); @@ -2129,6 +2134,24 @@ string serializeValuesXML(const gtsam::Values& values, string name); gtsam::Values* deserializeValuesXML(string serialized_values); gtsam::Values* deserializeValuesXML(string serialized_values, string name); +// Serialize +bool serializeGraphToFile(const gtsam::NonlinearFactorGraph& graph, string fname); +bool serializeGraphToXMLFile(const gtsam::NonlinearFactorGraph& graph, string fname); +bool serializeGraphToXMLFile(const gtsam::NonlinearFactorGraph& graph, string fname, string name); + +bool serializeValuesToFile(const gtsam::Values& values, string fname); +bool serializeValuesToXMLFile(const gtsam::Values& values, string fname); +bool serializeValuesToXMLFile(const gtsam::Values& values, string fname, string name); + +// Deserialize +gtsam::NonlinearFactorGraph* deserializeGraphToFile(string fname); +gtsam::NonlinearFactorGraph* deserializeGraphToXMLFile(string fname); +gtsam::NonlinearFactorGraph* deserializeGraphToXMLFile(string fname, string name); + +gtsam::Values* deserializeValuesToFile(string fname); +gtsam::Values* deserializeValuesToXMLFile(string fname); +gtsam::Values* deserializeValuesToXMLFile(string fname, string name); + //************************************************************************* // Utilities //************************************************************************* From a8199f2ed0e6ac9fec89bbf3cd5d0e635fb5369a Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 13 Jun 2013 14:46:53 +0000 Subject: [PATCH 44/47] Changed naming convention for deserialization functions --- gtsam.h | 12 ++++++------ gtsam/slam/serialization.cpp | 8 ++++---- gtsam/slam/serialization.h | 8 ++++---- gtsam/slam/tests/testSerialization.cpp | 8 ++++---- 4 files changed, 18 insertions(+), 18 deletions(-) diff --git a/gtsam.h b/gtsam.h index 40806c3a2..36a2a9304 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2144,13 +2144,13 @@ bool serializeValuesToXMLFile(const gtsam::Values& values, string fname); bool serializeValuesToXMLFile(const gtsam::Values& values, string fname, string name); // Deserialize -gtsam::NonlinearFactorGraph* deserializeGraphToFile(string fname); -gtsam::NonlinearFactorGraph* deserializeGraphToXMLFile(string fname); -gtsam::NonlinearFactorGraph* deserializeGraphToXMLFile(string fname, string name); +gtsam::NonlinearFactorGraph* deserializeGraphFromFile(string fname); +gtsam::NonlinearFactorGraph* deserializeGraphFromXMLFile(string fname); +gtsam::NonlinearFactorGraph* deserializeGraphFromXMLFile(string fname, string name); -gtsam::Values* deserializeValuesToFile(string fname); -gtsam::Values* deserializeValuesToXMLFile(string fname); -gtsam::Values* deserializeValuesToXMLFile(string fname, string name); +gtsam::Values* deserializeValuesFromFile(string fname); +gtsam::Values* deserializeValuesFromXMLFile(string fname); +gtsam::Values* deserializeValuesFromXMLFile(string fname, string name); //************************************************************************* // Utilities diff --git a/gtsam/slam/serialization.cpp b/gtsam/slam/serialization.cpp index e601a5e81..304b10b6d 100644 --- a/gtsam/slam/serialization.cpp +++ b/gtsam/slam/serialization.cpp @@ -275,7 +275,7 @@ bool gtsam::serializeValuesToXMLFile(const Values& values, } /* ************************************************************************* */ -NonlinearFactorGraph::shared_ptr gtsam::deserializeGraphToFile(const std::string& fname) { +NonlinearFactorGraph::shared_ptr gtsam::deserializeGraphFromFile(const std::string& fname) { NonlinearFactorGraph::shared_ptr result(new NonlinearFactorGraph()); if (!deserializeFromFile(fname, *result)) throw std::invalid_argument("Failed to open file" + fname); @@ -283,7 +283,7 @@ NonlinearFactorGraph::shared_ptr gtsam::deserializeGraphToFile(const std::string } /* ************************************************************************* */ -NonlinearFactorGraph::shared_ptr gtsam::deserializeGraphToXMLFile(const std::string& fname, +NonlinearFactorGraph::shared_ptr gtsam::deserializeGraphFromXMLFile(const std::string& fname, const std::string& name) { NonlinearFactorGraph::shared_ptr result(new NonlinearFactorGraph()); if (!deserializeFromXMLFile(fname, *result, name)) @@ -292,7 +292,7 @@ NonlinearFactorGraph::shared_ptr gtsam::deserializeGraphToXMLFile(const std::str } /* ************************************************************************* */ -Values::shared_ptr gtsam::deserializeValuesToFile(const std::string& fname) { +Values::shared_ptr gtsam::deserializeValuesFromFile(const std::string& fname) { Values::shared_ptr result(new Values()); if (!deserializeFromFile(fname, *result)) throw std::invalid_argument("Failed to open file" + fname); @@ -300,7 +300,7 @@ Values::shared_ptr gtsam::deserializeValuesToFile(const std::string& fname) { } /* ************************************************************************* */ -Values::shared_ptr gtsam::deserializeValuesToXMLFile(const std::string& fname, +Values::shared_ptr gtsam::deserializeValuesFromXMLFile(const std::string& fname, const std::string& name) { Values::shared_ptr result(new Values()); if (!deserializeFromXMLFile(fname, *result, name)) diff --git a/gtsam/slam/serialization.h b/gtsam/slam/serialization.h index 5d75f160c..08451fa0c 100644 --- a/gtsam/slam/serialization.h +++ b/gtsam/slam/serialization.h @@ -49,12 +49,12 @@ bool serializeValuesToXMLFile(const Values& values, const std::string& fname, const std::string& name = "values"); // Deserialize -NonlinearFactorGraph::shared_ptr deserializeGraphToFile(const std::string& fname); -NonlinearFactorGraph::shared_ptr deserializeGraphToXMLFile(const std::string& fname, +NonlinearFactorGraph::shared_ptr deserializeGraphFromFile(const std::string& fname); +NonlinearFactorGraph::shared_ptr deserializeGraphFromXMLFile(const std::string& fname, const std::string& name = "graph"); -Values::shared_ptr deserializeValuesToFile(const std::string& fname); -Values::shared_ptr deserializeValuesToXMLFile(const std::string& fname, +Values::shared_ptr deserializeValuesFromFile(const std::string& fname); +Values::shared_ptr deserializeValuesFromXMLFile(const std::string& fname, const std::string& name = "values"); } // \namespace gtsam diff --git a/gtsam/slam/tests/testSerialization.cpp b/gtsam/slam/tests/testSerialization.cpp index aad89d4a0..616b5b6b7 100644 --- a/gtsam/slam/tests/testSerialization.cpp +++ b/gtsam/slam/tests/testSerialization.cpp @@ -103,11 +103,11 @@ TEST( testSerialization, serialization_file ) { EXPECT(serializeValuesToXMLFile(values, path + "values.xml", "values1")); // Deserialize - NonlinearFactorGraph actGraph = *deserializeGraphToFile(path + "graph.dat"); - NonlinearFactorGraph actGraphXML = *deserializeGraphToXMLFile(path + "graph.xml", "graph1"); + NonlinearFactorGraph actGraph = *deserializeGraphFromFile(path + "graph.dat"); + NonlinearFactorGraph actGraphXML = *deserializeGraphFromXMLFile(path + "graph.xml", "graph1"); - Values actValues = *deserializeValuesToFile(path + "values.dat"); - Values actValuesXML = *deserializeValuesToXMLFile(path + "values.xml", "values1"); + Values actValues = *deserializeValuesFromFile(path + "values.dat"); + Values actValuesXML = *deserializeValuesFromXMLFile(path + "values.xml", "values1"); // Verify EXPECT(assert_equal(graph, actGraph)); From 46ea1d229df7435edc99ec00411f2193576a6bb3 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 13 Jun 2013 14:53:18 +0000 Subject: [PATCH 45/47] Added test for serializing to file in matlab, graph still fails --- matlab/gtsam_tests/testGraphValuesSerialization.m | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/matlab/gtsam_tests/testGraphValuesSerialization.m b/matlab/gtsam_tests/testGraphValuesSerialization.m index 1d4066c82..5583dde76 100644 --- a/matlab/gtsam_tests/testGraphValuesSerialization.m +++ b/matlab/gtsam_tests/testGraphValuesSerialization.m @@ -51,6 +51,15 @@ serialized_values = serializeValues(values); % returns a string deserializedValues = deserializeValues(serialized_values); % returns a new values CHECK('values.equals(deserializedValues)',values.equals(deserializedValues,1e-9)); -serialized_graph = serializeGraph(graph); % returns a string -deserializedGraph = deserializeGraph(serialized_graph); % returns a new graph -CHECK('graph.equals(deserializedGraph)',graph.equals(deserializedGraph,1e-9)); \ No newline at end of file +CHECK('serializeValuesToFile(values, values.dat)', serializeValuesToFile(values, 'values.dat')); +deserializedValuesFile = deserializeValuesFromFile('values.dat'); % returns a new values +CHECK('values.equals(deserializedValuesFile)',values.equals(deserializedValuesFile,1e-9)); + +% % FAIL: unregistered class - derived class not registered or exported +% serialized_graph = serializeGraph(graph); % returns a string +% deserializedGraph = deserializeGraph(serialized_graph); % returns a new graph +% CHECK('graph.equals(deserializedGraph)',graph.equals(deserializedGraph,1e-9)); +% +% CHECK('serializeGraphToFile(graph, graph.dat)', serializeGraphToFile(graph, 'graph.dat')); +% deserializedGraphFile = deserializeGraphFromFile('graph.dat'); % returns a new graph +% CHECK('graph.equals(deserializedGraphFile)',graph.equals(deserializedGraphFile,1e-9)); \ No newline at end of file From fd42854222323a4f291f7652e5a529f5669f6544 Mon Sep 17 00:00:00 2001 From: Vadim Indelman Date: Thu, 13 Jun 2013 21:17:21 +0000 Subject: [PATCH 46/47] Enabled R matrix in noise model. --- gtsam.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam.h b/gtsam.h index 36a2a9304..45c701ee1 100644 --- a/gtsam.h +++ b/gtsam.h @@ -985,7 +985,7 @@ virtual class Base { virtual class Gaussian : gtsam::noiseModel::Base { static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); static gtsam::noiseModel::Gaussian* Covariance(Matrix R); - //Matrix R() const; // FIXME: cannot parse!!! + Matrix R() const; bool equals(gtsam::noiseModel::Base& expected, double tol); void print(string s) const; }; @@ -994,7 +994,7 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian { static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); static gtsam::noiseModel::Diagonal* Variances(Vector variances); static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); -// Matrix R() const; // FIXME: cannot parse!!! + Matrix R() const; void print(string s) const; }; From d85c773f7cbc1eddd6c2640a8791a77387276726 Mon Sep 17 00:00:00 2001 From: Kyel Ok Date: Fri, 14 Jun 2013 21:18:22 +0000 Subject: [PATCH 47/47] Changes in AHRS - flat trim initialization added, some comments by Frank, aidingAvailablitiy function --- gtsam_unstable/slam/AHRS.cpp | 23 +++++++++++++-- gtsam_unstable/slam/AHRS.h | 5 +++- gtsam_unstable/slam/Mechanization_bRn2.cpp | 34 +++++++++++++--------- gtsam_unstable/slam/Mechanization_bRn2.h | 7 +++-- 4 files changed, 50 insertions(+), 19 deletions(-) diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index cbd1096a9..002c9e3e6 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -24,11 +24,12 @@ Matrix I3 = eye(3); Matrix Z3 = zeros(3, 3); /* ************************************************************************* */ -AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e) : +AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, + bool flat) : KF_(9) { // Initial state - mech0_ = Mechanization_bRn2::initialize(stationaryU, stationaryF, g_e); + mech0_ = Mechanization_bRn2::initialize(stationaryU, stationaryF, g_e, flat); size_t T = stationaryU.cols(); @@ -145,6 +146,18 @@ std::pair AHRS::integrate( return make_pair(newMech, newState); } +/* ************************************************************************* */ +bool AHRS::isAidingAvailable(const Mechanization_bRn2& mech, + const gtsam::Vector& f, const gtsam::Vector& u, double ge) { + + // Subtract the biases + Vector f_ = f - mech.x_a(); + Vector u_ = u - mech.x_g(); + + double mu_f = f_.norm() - ge; + double mu_u = u_.norm(); + return (fabs(mu_f)<0.5 && mu_u < 5.0 / 180.0 * 3.1415926); +} /* ************************************************************************* */ std::pair AHRS::aid( @@ -169,10 +182,14 @@ std::pair AHRS::aid( // F(:,k) = mech.x_a + dx_a - bRn*n_g; // F(:,k) = mech.x_a + dx_a - bRn*(I+P)*n_g; // F(:,k) = mech.x_a + dx_a - b_g - bRn*(rho x n_g); // P = [rho]_x - // b_g - (mech.x_a - F(:,k)) = dx_a + bRn*(n_g x rho); + // Hence, the measurement z = b_g - (mech.x_a - F(:,k)) is predicted + // from the filter state (dx_a, rho) as dx_a + bRn*(n_g x rho) + // z = b_g - (mech.x_a - F(:,k)) = dx_a + bRn*(n_g x rho) z = bRn * n_g_ - measured_b_g; + // Now the Jacobian H Matrix b_g = bRn * n_g_cross_; H = collect(3, &b_g, &Z3, &I3); + // And the measurement noise, TODO: should be created once where sigmas_v_a is given R = diag(emul(sigmas_v_a_, sigmas_v_a_)); } diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index 01c61704b..1d7eb85f5 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -43,7 +43,7 @@ public: * @param stationaryF initial interval of accelerator measurements, 3xn matrix * @param g_e if given, initializes gravity with correct value g_e */ - AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e); + AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, bool flat=false); std::pair initialize(double g_e); @@ -51,6 +51,9 @@ public: const Mechanization_bRn2& mech, KalmanFilter::State state, const Vector& u, double dt); + bool isAidingAvailable(const Mechanization_bRn2& mech, + const Vector& f, const Vector& u, double ge); + std::pair aid( const Mechanization_bRn2& mech, KalmanFilter::State state, const Vector& f, bool Farrell=0); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index ca95843af..312814d15 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -12,35 +12,43 @@ namespace gtsam { /* ************************************************************************* */ Mechanization_bRn2 Mechanization_bRn2::initializeVector(const std::list& U, - const std::list& F, const double g_e) { + const std::list& F, const double g_e, bool flat) { Matrix Umat = Matrix_(3,U.size(), concatVectors(U)); Matrix Fmat = Matrix_(3,F.size(), concatVectors(F)); - return initialize(Umat, Fmat, g_e); + return initialize(Umat, Fmat, g_e, flat); } /* ************************************************************************* */ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, - const Matrix& F, const double g_e) { + const Matrix& F, const double g_e, bool flat) { // estimate gyro bias by averaging gyroscope readings (10.62) Vector x_g = U.rowwise().mean(); - Vector meanF = -F.rowwise().mean(); + Vector meanF = F.rowwise().mean(); - Vector b_g, x_a; + // estimate gravity + Vector b_g; if(g_e == 0) { - // estimate gravity in body frame from accelerometer (10.13) - b_g = meanF; - // estimate accelerometer bias as zero (10.65) - x_a = zero(3); - + if (flat) + // acceleration measured is along the z-axis. + b_g = Vector_(3, 0.0, 0.0, norm_2(meanF)); + else + // acceleration measured is the opposite of gravity (10.13) + b_g = -meanF; } else { - // normalize b_g and attribute remainder to biases - b_g = g_e * meanF/meanF.norm(); - x_a = -(meanF - b_g); + if (flat) + // gravity is downward along the z-axis since we are flat on the ground + b_g = Vector_(3,0.0,0.0,g_e); + else + // normalize b_g and attribute remainder to biases + b_g = - g_e * meanF/meanF.norm(); } + // estimate accelerometer bias + Vector x_a = meanF + b_g; + // initialize roll, pitch (eqns. 10.14, 10.15) double g1=b_g(0); double g2=b_g(1); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index b81e685d4..711a44bf9 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -47,12 +47,15 @@ public: * Initialize the first Mechanization state * @param U a list of gyro measurement vectors * @param F a list of accelerometer measurement vectors + * @param g_e gravity magnitude + * @param flat flag saying whether this is a flat trim init */ static Mechanization_bRn2 initializeVector(const std::list& U, - const std::list& F, const double g_e = 0); + const std::list& F, const double g_e = 0, bool flat=false); + /// Matrix version of initialize static Mechanization_bRn2 initialize(const Matrix& U, - const Matrix& F, const double g_e = 0); + const Matrix& F, const double g_e = 0, bool flat=false); /** * Correct AHRS full state given error state dx