diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 575ee427f..e7270cafa 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -6,7 +6,7 @@ set (gtsam_unstable_subdirs discrete # linear dynamics -# nonlinear + nonlinear slam navigation ) diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp new file mode 100644 index 000000000..a3e063da1 --- /dev/null +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -0,0 +1,563 @@ +/* ---------------------------------------------------------------------------- + + * 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 BatchFixedLagSmoother.cpp + * @brief An LM-based fixed-lag smoother. To the extent possible, this class mimics the iSAM2 + * interface. However, additional parameters, such as the smoother lag and the timestamp associated + * with each variable are needed. + * + * @author Michael Kaess, Stephen Williams + * @date Oct 14, 2012 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { + std::cout << label; + BOOST_FOREACH(gtsam::Key key, keys) { + std::cout << " " << gtsam::DefaultKeyFormatter(key); + } + std::cout << std::endl; +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering) { + std::cout << "f("; + BOOST_FOREACH(Index index, factor->keys()) { + std::cout << " " << index << "[" << gtsam::DefaultKeyFormatter(ordering.key(index)) << "]"; + } + std::cout << " )" << std::endl; +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label) { + std::cout << label << std::endl; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { + PrintSymbolicFactor(factor, ordering); + } +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor) { + std::cout << "f("; + if(factor) { + BOOST_FOREACH(Key key, factor->keys()) { + std::cout << " " << gtsam::DefaultKeyFormatter(key); + } + } else { + std::cout << " NULL"; + } + std::cout << " )" << std::endl; +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label) { + std::cout << label << std::endl; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, graph) { + PrintSymbolicFactor(factor); + } +} + + + + +/* ************************************************************************* */ +BatchFixedLagSmoother::BatchFixedLagSmoother(const LevenbergMarquardtParams& params, double smootherLag, bool enforceConsistency) : + parameters_(params), smootherLag_(smootherLag), enforceConsistency_(enforceConsistency) { +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { + std::cout << s; +} + +/* ************************************************************************* */ +bool BatchFixedLagSmoother::equals(const BatchFixedLagSmoother& rhs, double tol) const { + return factors_.equals(rhs.factors_, tol) + && theta_.equals(rhs.theta_, tol) + && std::fabs(smootherLag_ - rhs.smootherLag_) < tol + && std::equal(timestampKeyMap_.begin(), timestampKeyMap_.end(), rhs.timestampKeyMap_.begin()); +} + +/* ************************************************************************* */ +Matrix BatchFixedLagSmoother::marginalR(Key key) const { +// TODO: Fix This +// // Use iSAM2 object to get the marginal factor +// GaussianFactor::shared_ptr marginalGaussian; +// if(params().getFactorization() == "QR") +// marginalGaussian = isam_.marginalFactor(getOrdering().at(key), EliminateQR); +// else if(params().getFactorization() == "CHOLESKY") +// marginalGaussian = isam_.marginalFactor(getOrdering().at(key), EliminateCholesky); +// else +// throw std::invalid_argument(boost::str(boost::format("Encountered unknown factorization type of '%s'. Known types are 'QR' and 'CHOLESKY'.") % params().getFactorization())); +// +// // Extract the information matrix +// JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast(marginalGaussian); +// assert(marginalJacobian != 0); +// return marginalJacobian->getA(marginalJacobian->begin()); + return Matrix(); +} + +/* ************************************************************************* */ +Matrix BatchFixedLagSmoother::marginalInformation(Key key) const { + Matrix R(marginalR(key)); + return R.transpose() * R; +} + +/* ************************************************************************* */ +Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const { + Matrix Rinv(inverse(marginalR(key))); + return Rinv * Rinv.transpose(); +} + +/* ************************************************************************* */ +BatchFixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { + + const bool debug = ISDEBUG("BatchFixedLagSmoother update"); + if(debug) { + std::cout << "BatchFixedLagSmoother::update() START" << std::endl; + } + + // Add the new factors + updateFactors(newFactors); + + // Add the new variables + theta_.insert(newTheta); + + // Update the Timestamps associated with the factor keys + updateKeyTimestampMap(timestamps); + + // Get current timestamp + double current_timestamp = getCurrentTimestamp(); + if(debug) std::cout << "Current Timestamp: " << current_timestamp << std::endl; + + // Find the set of variables to be marginalized out + std::set marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_); + if(debug) { + std::cout << "Marginalizable Keys: "; + BOOST_FOREACH(Key key, marginalizableKeys) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << std::endl; + } + + // Marginalize out these variables. + // This removes any factors that touch marginalized variables and adds new linear(ized) factors to the graph + marginalizeKeys(marginalizableKeys); + + // Create the optimizer + Values linpoint; + linpoint.insert(theta_); + if(enforceConsistency_ && linearizedKeys_.size() > 0) { + linpoint.update(linearizedKeys_); + } + LevenbergMarquardtOptimizer optimizer(factors_, linpoint, parameters_); + + // Use a custom optimization loop so the linearization points can be controlled + double currentError; + do { + // Do next iteration + currentError = optimizer.error(); + optimizer.iterate(); + + // Force variables associated with linearized factors to keep the same linearization point + if(enforceConsistency_ && linearizedKeys_.size() > 0) { + // Put the old values of the linearized keys back into the optimizer state + optimizer.state().values.update(linearizedKeys_); + optimizer.state().error = factors_.error(optimizer.state().values); + } + + // Maybe show output + if(parameters_.verbosity >= NonlinearOptimizerParams::VALUES) optimizer.values().print("newValues"); + if(parameters_.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "newError: " << optimizer.error() << std::endl; + } while(optimizer.iterations() < parameters_.maxIterations && + !checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol, + parameters_.errorTol, currentError, optimizer.error(), parameters_.verbosity)); + + // Update the Values from the optimizer + theta_ = optimizer.values(); + + // Create result structure + Result result; + result.iterations = optimizer.state().iterations; + result.linearVariables = linearizedKeys_.size(); + result.nonlinearVariables = theta_.size() - linearizedKeys_.size(); + result.error = optimizer.state().error; + + if(debug) { + std::cout << "BatchFixedLagSmoother::update() FINISH" << std::endl; + } + + return result; +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::updateFactors(const NonlinearFactorGraph& newFactors) { + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { + Index index; + // Insert the factor into an existing hole in the factor graph, if possible + if(availableSlots_.size() > 0) { + index = availableSlots_.front(); + availableSlots_.pop(); + factors_.replace(index, factor); + } else { + index = factors_.size(); + factors_.push_back(factor); + } + // Update the FactorIndex + BOOST_FOREACH(Key key, *factor) { + factorIndex_[key].insert(index); + } + } +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::removeFactors(const std::set& deleteFactors) { + BOOST_FOREACH(size_t slot, deleteFactors) { + if(factors_.at(slot)) { + // Remove references to this factor from the FactorIndex + BOOST_FOREACH(Key key, *(factors_.at(slot))) { + factorIndex_[key].erase(slot); + } + // Remove the factor from the factor graph + factors_.remove(slot); + // Add the factor's old slot to the list of available slots + availableSlots_.push(slot); + } else { + // TODO: Throw an error?? + std::cout << "Attempting to remove a factor from slot " << slot << ", but it is already NULL." << std::endl; + } + } +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps) { + // Loop through each key and add/update it in the map + BOOST_FOREACH(const KeyTimestampMap::value_type& key_timestamp, timestamps) { + // Check to see if this key already exists inthe database + KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first); + + // If the key already exists + if(keyIter != keyTimestampMap_.end()) { + // Find the entry in the Timestamp-Key database + std::pair range = timestampKeyMap_.equal_range(keyIter->second); + TimestampKeyMap::iterator timeIter = range.first; + while(timeIter->second != key_timestamp.first) { + ++timeIter; + } + // remove the entry in the Timestamp-Key database + timestampKeyMap_.erase(timeIter); + // insert an entry at the new time + timestampKeyMap_.insert(TimestampKeyMap::value_type(key_timestamp.second, key_timestamp.first)); + // update the Key-Timestamp database + keyIter->second = key_timestamp.second; + } else { + // Add the Key-Timestamp database + keyTimestampMap_.insert(key_timestamp); + // Add the key to the Timestamp-Key database + timestampKeyMap_.insert(TimestampKeyMap::value_type(key_timestamp.second, key_timestamp.first)); + } + } +} + +/* ************************************************************************* */ +double BatchFixedLagSmoother::getCurrentTimestamp() const { + if(timestampKeyMap_.size() > 0) { + return timestampKeyMap_.rbegin()->first; + } else { + return -std::numeric_limits::max(); + } +} + +/* ************************************************************************* */ +std::set BatchFixedLagSmoother::findKeysBefore(double timestamp) const { + std::set keys; + TimestampKeyMap::const_iterator end = timestampKeyMap_.lower_bound(timestamp); + for(TimestampKeyMap::const_iterator iter = timestampKeyMap_.begin(); iter != end; ++iter) { + keys.insert(iter->second); + } + return keys; +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::eraseKeys(const std::set& keys) { + +// bool debug = true; + +// if(debug) std::cout << "BatchFixedLagSmoother::eraseKeys() START" << std::endl; + +// if(debug) PrintKeySet(keys, "Keys To Erase: "); + + BOOST_FOREACH(Key key, keys) { +// if(debug) std::cout << "Attempting to erase key " << DefaultKeyFormatter(key) << std::endl; + + // Erase the key from the Timestamp->Key map + double timestamp = keyTimestampMap_.at(key); + +// if(debug) std::cout << "Timestamp associated with key: " << timestamp << std::endl; + + TimestampKeyMap::iterator iter = timestampKeyMap_.lower_bound(timestamp); + while(iter != timestampKeyMap_.end() && iter->first == timestamp) { + if(iter->second == key) { + +// if(debug) { +// std::cout << "Contents of TimestampKeyMap before Erase:" << std::endl; +// BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) { +// std::cout << " " << timestamp_key.first << " -> " << DefaultKeyFormatter(timestamp_key.second) << std::endl; +// } +// } + + timestampKeyMap_.erase(iter++); + +// if(debug) { +// std::cout << "Contents of TimestampKeyMap before Erase:" << std::endl; +// BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) { +// std::cout << " " << timestamp_key.first << " -> " << DefaultKeyFormatter(timestamp_key.second) << std::endl; +// } +// } + +// if(debug) std::cout << "Erased 1 entry from timestampKeyMap_" << std::endl; + } else { + ++iter; + } + } + // Erase the key from the Key->Timestamp map + size_t ret; + ret = keyTimestampMap_.erase(key); +// if(debug) std::cout << "Erased " << ret << " entries from keyTimestampMap_" << std::endl; + + // Erase the key from the values + theta_.erase(key); +// if(debug) std::cout << "(Hopefully) Erased 1 entries from theta_" << std::endl; + + // Erase the key from the factor index + ret = factorIndex_.erase(key); +// if(debug) std::cout << "Erased " << ret << " entries from factorIndex_" << std::endl; + + // Erase the key from the set of linearized keys + if(linearizedKeys_.exists(key)) { + linearizedKeys_.erase(key); +// if(debug) std::cout << "Erased 1 entry from linearizedKeys_" << std::endl; + } + } + +// if(debug) std::cout << "BatchFixedLagSmoother::eraseKeys() FINISH" << std::endl; +} + +/* ************************************************************************* */ +struct FactorTree { + std::set factors; + std::set keys; + + FactorTree(const std::set& factors, const NonlinearFactorGraph& allFactors) : factors(factors) { + BOOST_FOREACH(const Index& factor, factors) { + BOOST_FOREACH(Key key, *(allFactors.at(factor))) { + keys.insert(key); + } + } + }; + + void push_back(const FactorTree& factorTree) { + factors.insert(factorTree.factors.begin(), factorTree.factors.end()); + keys.insert(factorTree.keys.begin(), factorTree.keys.end()); + } + + bool hasCommonKeys(Index factor, const NonlinearFactorGraph& allFactors) { + const NonlinearFactor::shared_ptr& f = allFactors.at(factor); + std::set::const_iterator iter = std::find_first_of(keys.begin(), keys.end(), f->begin(), f->end()); + return iter != keys.end(); + } + + template + bool hasCommonKeys(ForwardIterator first, ForwardIterator last, const NonlinearFactorGraph& allFactors) { + for(ForwardIterator factor = first; factor != last; ++factor) { + if(hasCommonKeys(*factor, allFactors)) + return true; + } + return false; + } +}; + +/* ************************************************************************* */ +void BatchFixedLagSmoother::marginalizeKeys(const std::set& marginalizableKeys) { + + const bool debug = ISDEBUG("BatchFixedLagSmoother update"); + if(debug) std::cout << "BatchFixedLagSmoother::marginalizeKeys() START" << std::endl; + + + // In order to marginalize out the selected variables, the factors involved in those variables + // must be identified and removed from iSAM2. Also, the effect of those removed factors on the + // remaining variables needs to be accounted for. This will be done with linear(ized) factors from + // a partial clique marginalization (or from the iSAM2 cached factor if the entire clique is removed). + // This function finds the set of factors to be removed and generates the linearized factors that + // must be added. + + if(marginalizableKeys.size() > 0) { + + if(debug) PrintKeySet(marginalizableKeys, "Marginalizable Keys:"); + + // Find all of the factors associated with marginalizable variables. This set of factors may form a forest. + typedef std::list FactorForest; + FactorForest factorForest; + BOOST_FOREACH(Key key, marginalizableKeys) { + + if(debug) std::cout << "Looking for factors involving key " << DefaultKeyFormatter(key) << std::endl; + + // Get the factors associated with this variable + const std::set& factors = factorIndex_.at(key); + + if(debug) { std::cout << "Found the following factors:" << std::endl; BOOST_FOREACH(size_t i, factors) { std::cout << " "; PrintSymbolicFactor(factors_.at(i)); } } + + // Loop over existing factor trees, looking for common keys + std::vector commonTrees; + for(FactorForest::iterator tree = factorForest.begin(); tree != factorForest.end(); ++tree) { + if(tree->hasCommonKeys(factors.begin(), factors.end(), factors_)) { + commonTrees.push_back(tree); + } + } + + if(debug) std::cout << "Found " << commonTrees.size() << " common trees." << std::endl; + + if(commonTrees.size() == 0) { + // No common trees were found. Create a new one. + factorForest.push_back(FactorTree(factors, factors_)); + + if(debug) std::cout << "Created a new tree." << std::endl; + + } else { + // Extract the last common tree + FactorForest::iterator commonTree = commonTrees.back(); + commonTrees.pop_back(); + // Merge the current factors into this tree + commonTree->push_back(FactorTree(factors, factors_)); + // Merge all other common trees into this one, deleting the other trees from the forest. + BOOST_FOREACH(FactorForest::iterator& tree, commonTrees) { + commonTree->push_back(*tree); + factorForest.erase(tree); + } + } + } + + if(debug) std::cout << "Found " << factorForest.size() << " factor trees in the set of removed factors." << std::endl; + + // For each tree in the forest: + // (0) construct an ordering for the tree + // (1) construct a linear factor graph + // (2) solve for the marginal factors + // (3) convert the marginal factors into Linearized Factors + // (4) remove the marginalized factors from the graph + // (5) add the factors in this tree to the graph + BOOST_FOREACH(const FactorTree& factorTree, factorForest) { + // (0) construct an ordering for this tree + // The ordering should place the marginalizable keys first, then the remaining keys + Ordering ordering; + + std::set marginalizableTreeKeys; + std::set_intersection(factorTree.keys.begin(), factorTree.keys.end(), + marginalizableKeys.begin(), marginalizableKeys.end(), + std::inserter(marginalizableTreeKeys, marginalizableTreeKeys.end())); + + std::set remainingTreeKeys; + std::set_difference(factorTree.keys.begin(), factorTree.keys.end(), + marginalizableTreeKeys.begin(), marginalizableTreeKeys.end(), + std::inserter(remainingTreeKeys, remainingTreeKeys.end())); + + // TODO: It may be worthwhile to use CCOLAMD here. (but maybe not???) + BOOST_FOREACH(Key key, marginalizableTreeKeys) { + ordering.push_back(key); + } + BOOST_FOREACH(Key key, remainingTreeKeys) { + ordering.push_back(key); + } + + // (1) construct a linear factor graph + GaussianFactorGraph graph; + BOOST_FOREACH(size_t factor, factorTree.factors) { + graph.push_back( factors_.at(factor)->linearize(theta_, ordering) ); + } + + if(debug) PrintSymbolicGraph(graph, ordering, "Factor Tree:"); + + // (2) solve for the marginal factors + // Perform partial elimination, resulting in a conditional probability ( P(MarginalizedVariable | RemainingVariables) + // and factors on the remaining variables ( f(RemainingVariables) ). These are the factors we need to add to iSAM2 + std::vector variables; + BOOST_FOREACH(Key key, marginalizableTreeKeys) { + variables.push_back(ordering.at(key)); + } + std::pair result = graph.eliminate(variables); + graph = result.second; + + if(debug) PrintSymbolicGraph(graph, ordering, "Factors on Remaining Variables:"); + + // (3) convert the marginal factors into Linearized Factors + NonlinearFactorGraph newFactors; + BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, graph) { + // These factors are all generated from BayesNet conditionals. They should all be Jacobians. + JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast(gaussianFactor); + assert(jacobianFactor); + LinearizedGaussianFactor::shared_ptr factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(jacobianFactor, ordering, getLinearizationPoint())); + // add it to the new factor set + newFactors.push_back(factor); + } + + if(debug) std::cout << "1" << std::endl; + + // (4) remove the marginalized factors from the graph + removeFactors(factorTree.factors); + + if(debug) std::cout << "2" << std::endl; + + // (5) add the factors in this tree to the main set of factors + updateFactors(newFactors); + + if(debug) std::cout << "3" << std::endl; + + // (6) add the keys involved in the linear(ized) factors to the linearizedKey list + FastSet linearizedKeys = newFactors.keys(); + BOOST_FOREACH(Key key, linearizedKeys) { + if(!linearizedKeys_.exists(key)) { + linearizedKeys_.insert(key, theta_.at(key)); + } + } + + if(debug) std::cout << "4" << std::endl; + } + + if(debug) std::cout << "5" << std::endl; + + // Store the final estimate of each marginalized key + BOOST_FOREACH(Key key, marginalizableKeys) { + thetaFinal_.insert(key, theta_.at(key)); + } + + // Remove the marginalized keys from the smoother data structures + eraseKeys(marginalizableKeys); + + if(debug) std::cout << "6" << std::endl; + } + + if(debug) std::cout << "BatchFixedLagSmoother::marginalizeKeys() FINISH" << std::endl; +} + +/* ************************************************************************* */ +} /// namespace gtsam diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h new file mode 100644 index 000000000..ec7dc9cb6 --- /dev/null +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -0,0 +1,227 @@ +/* ---------------------------------------------------------------------------- + + * 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 BatchFixedLagSmoother.h + * @brief An LM-based fixed-lag smoother. To the extent possible, this class mimics the iSAM2 + * interface. However, additional parameters, such as the smoother lag and the timestamp associated + * with each variable are needed. + * + * @author Michael Kaess, Stephen Williams + * @date Oct 14, 2012 + */ + +// \callgraph +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +class BatchFixedLagSmoother { + +public: + + /// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother + typedef boost::shared_ptr shared_ptr; + + /// Typedef for a Key-Timestamp map/database + typedef std::map KeyTimestampMap; + typedef std::multimap TimestampKeyMap; + + /** + * Meta information returned about the update + */ + // TODO: Think of some more things to put here + struct Result { + size_t iterations; ///< The number of optimizer iterations performed + size_t nonlinearVariables; ///< The number of variables that can be relinearized + size_t linearVariables; ///< The number of variables that must keep a constant linearization point + double error; ///< The final factor graph error + Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {}; + }; + + + /** default constructor */ + BatchFixedLagSmoother(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), double smootherLag = 0.0, bool enforceConsistency = false); + + /** destructor */ + virtual ~BatchFixedLagSmoother() { }; + + // TODO: Create a better 'print' + /** Print the factor for debugging and testing (implementing Testable) */ + virtual void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** Check if two IncrementalFixedLagSmoother Objects are equal */ + virtual bool equals(const BatchFixedLagSmoother& rhs, double tol = 1e-9) const; + + /** + * Add new factors, updating the solution and relinearizing as needed. + */ + Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const KeyTimestampMap& timestamps = KeyTimestampMap()); + + /** Access the current set of timestamps associated with each variable */ + const KeyTimestampMap& getTimestamps() const { + return keyTimestampMap_; + } + + /** Access the current linearization point */ + const Values& getLinearizationPoint() const { + return theta_; + } + + /** Compute an estimate from the incomplete linear delta computed during the last update. + * This delta is incomplete because it was not updated below wildfire_threshold. If only + * a single variable is needed, it is faster to call calculateEstimate(const KEY&). + */ + Values calculateEstimate() const { + //return theta_.retract(delta_, ordering_); + return theta_; + } + + /** Compute an estimate for a single variable using its incomplete linear delta computed + * during the last update. This is faster than calling the no-argument version of + * calculateEstimate, which operates on all variables. + * @param key + * @return + */ + template + VALUE calculateEstimate(Key key) const { + //const Index index = ordering_.at(key); + //const SubVector delta = delta_.at(index); + //return theta_.at(key).retract(delta); + return theta_.at(key); + } + + /** Compute an estimate using a complete delta computed by a full back-substitution. + */ + Values calculateBestEstimate() const { + return calculateEstimate(); + } + + /** Return the final/best estimate of each variable encountered, including those that have been marginalized out previously. + */ + Values calculateFinalEstimate() const { + Values final(thetaFinal_); + final.insert(theta_); + return final; + } + + + /** Access the set of nonlinear factors */ + const NonlinearFactorGraph& getFactorsUnsafe() const { + return factors_; + } + + /** return the current set of iSAM2 parameters */ + const LevenbergMarquardtParams& params() const { + return parameters_; + } + + /** return the current smoother lag */ + double smootherLag() const { + return smootherLag_; + } + + + + /** return the marginal square root information matrix associated with a specific variable */ + Matrix marginalR(Key key) const; + + /** return the marginal information matrix associated with a specific variable */ + Matrix marginalInformation(Key key) const; + + /** return the marginal covariance associated with a specific variable */ + Matrix marginalCovariance(Key key) const; + + + +protected: + + /** The nonlinear factors **/ + NonlinearFactorGraph factors_; + + /** The current linearization point **/ + Values theta_; + + /** The final estimate of each variable **/ + Values thetaFinal_; + + /** The L-M optimization parameters **/ + LevenbergMarquardtParams parameters_; + + /** The length of the smoother lag. Any variable older than this amount will be marginalized out. */ + double smootherLag_; + + /** A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the + * linearization point of all variables involved in linearized/marginal factors at the edge of the + * smoothing window. This idea is from ??? TODO: Look up paper reference **/ + bool enforceConsistency_; + + /** The current timestamp associated with each tracked key */ + TimestampKeyMap timestampKeyMap_; + KeyTimestampMap keyTimestampMap_; + + /** A typedef defining an Key-Factor mapping **/ + typedef std::map > FactorIndex; + + /** A cross-reference structure to allow efficient factor lookups by key **/ + FactorIndex factorIndex_; + + /** The set of keys involved in current linearized factors. These keys should not be relinearized. **/ + //std::set linearizedKeys_; + Values linearizedKeys_; + + /** The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes. **/ + std::queue availableSlots_; + + + /** Augment the list of factors with a set of new factors */ + void updateFactors(const NonlinearFactorGraph& newFactors); + + /** Remove factors from the list of factors by slot index */ + void removeFactors(const std::set& deleteFactors); + + /** Update the Timestamps associated with the keys */ + void updateKeyTimestampMap(const KeyTimestampMap& newTimestamps); + + /** Find the most recent timestamp of the system */ + double getCurrentTimestamp() const; + + /** Find all of the keys associated with timestamps before the provided time */ + std::set findKeysBefore(double timestamp) const; + + /** Erase any keys associated with timestamps before the provided time */ + void eraseKeys(const std::set& keys); + + /** Marginalize out selected variables */ + void marginalizeKeys(const std::set& marginalizableKeys); + + +private: + + static void PrintKeySet(const std::set& keys, const std::string& label); + + static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering); + + static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label); + + static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor); + + static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label); + +}; // BatchFixedLagSmoother + +} /// namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp new file mode 100644 index 000000000..4d0c70653 --- /dev/null +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -0,0 +1,916 @@ +/* ---------------------------------------------------------------------------- + + * 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 ConcurrentBatchFilter.cpp + * @brief A Levenberg-Marquardt Batch Filter that implements the + * Concurrent Filtering and Smoothing interface. + * @author Stephen Williams + */ + +#include +#include +#include +#include +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +void ConcurrentBatchFilter::SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent) { + std::cout << indent << "P( "; + BOOST_FOREACH(Index index, clique->conditional()->frontals()){ + std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; + } + if(clique->conditional()->nrParents() > 0) { + std::cout << "| "; + } + BOOST_FOREACH(Index index, clique->conditional()->parents()){ + std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; + } + std::cout << ")" << std::endl; + + BOOST_FOREACH(const Clique& child, clique->children()) { + SymbolicPrintTree(child, ordering, indent+" "); + } +} + +/* ************************************************************************* */ +void ConcurrentBatchFilter::print(const std::string& s, const KeyFormatter& keyFormatter) const { + std::cout << s; + graph_.print("Factors:\n"); + theta_.print("Values:\n"); +} + +/* ************************************************************************* */ +ConcurrentBatchFilter::Result ConcurrentBatchFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { + + gttic(update); + + // Create the return result meta-data + Result result; + + gttic(augment_system); + // Add the new factors to the graph + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { + insertFactor(factor); + } + // Add the new variables to theta + theta_.insert(newTheta); + // Update the Timestamps associated with the factor keys + updateKeyTimestampMap(timestamps); + gttoc(augment_system); + + // Optimize the graph, updating theta + gttic(optimize); + if(graph_.size() > 0) { + // Create an L-M optimizer + Values linpoint; + linpoint.insert(theta_); + if(separatorValues_.size() > 0) { + linpoint.update(separatorValues_); + } + LevenbergMarquardtOptimizer optimizer(graph_, linpoint, parameters_); + + // Use a custom optimization loop so the linearization points can be controlled + double currentError; + do { + // Do next iteration + gttic(optimizer_iteration); + currentError = optimizer.error(); + optimizer.iterate(); + gttoc(optimizer_iteration); + + // Force variables associated with root keys to keep the same linearization point + gttic(enforce_consistency); + if(separatorValues_.size() > 0) { + // Put the old values of the root keys back into the optimizer state + optimizer.state().values.update(separatorValues_); + optimizer.state().error = graph_.error(optimizer.state().values); + } + gttoc(enforce_consistency); + + // Maybe show output + if(parameters_.verbosity >= NonlinearOptimizerParams::VALUES) optimizer.values().print("newValues"); + if(parameters_.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "newError: " << optimizer.error() << std::endl; + } while(optimizer.iterations() < parameters_.maxIterations && + !checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol, + parameters_.errorTol, currentError, optimizer.error(), parameters_.verbosity)); + + // Update the Values from the optimizer + theta_ = optimizer.values(); + + result.iterations = optimizer.state().iterations; + result.nonlinearVariables = theta_.size() - separatorValues_.size(); + result.linearVariables = separatorValues_.size(); + result.error = optimizer.state().error; + } + gttoc(optimize); + + gttoc(update); + + return result; +} + + + + +/* ************************************************************************* */ +void ConcurrentBatchFilter::presync() { + + gttic(presync); + + gttoc(presync); +} + +/* ************************************************************************* */ +void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFactors) { + gttic(synchronize); + +std::cout << "ConcurrentBatchFilter::synchronize(*) Begin" << std::endl; + +//// Check that solution before marginalization matches solution after marginalization +//Values solutionBefore; +//solutionBefore.insert(theta_); +//solutionBefore.update(smootherSeparatorValues_); +//Ordering orderingBefore = *graph_.orderingCOLAMD(solutionBefore); +//GaussianFactorGraph lingraphBefore = *graph_.linearize(solutionBefore, orderingBefore); +//GaussianMultifrontalSolver gmsBefore(lingraphBefore, true); +//VectorValues deltaBefore = *gmsBefore.optimize(); +//solutionBefore = solutionBefore.retract(deltaBefore, orderingBefore); + + +std::cout << "ConcurrentBatchFilter::synchronize(*) Input Smoother Summarization:" << std::endl; +BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, summarizedFactors) { + std::cout << " f( "; + BOOST_FOREACH(Key key, factor->keys()) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << ")" << std::endl; +} + + // At this step, we need to update the summarized factors representing the smoother, + // then update the solution by re-eliminating the nonlinear graph. In the process, we + // will create a Bayes Tree and form a new root clique. We will let CCOLAMD define + // the ordering, constraining the most recent variables (variables > lag) to the + // top of the tree. + // + // Once formed, we then need to determine which branches will be sent to the smoother, + // which branches constitute the filter, and which clique(s) form the root. + // + // Ideally, all of the old variables will be in the smoother. But, depending on the + // factors and the ordering, there may be some cliques with recent and old variables. + // So, we define the smoother branches as those remaining after removing any clique + // with any recent variables. + // + // The root should be the smallest set of variables that separate the smoother from + // the rest of the graph. Thus, the root will be the parents of all the smoother + // branches (plus the path to the root to ensure a connected, full-rank system). + // + // The filter branches are then defined as the children of the root that are not + // smoother branches. + + +//std::cout << "Previous Graph:" << std::endl; +//for(size_t i = 0; i < graph_.size(); ++i) { +// if(graph_[i]) { +// std::cout << " f" << i << "( "; +// BOOST_FOREACH(Key key, graph_[i]->keys()) { +// std::cout << DefaultKeyFormatter(key) << " "; +// } +// std::cout << ")" << std::endl; +// } +//} + +// TODO: Temporarily remove smoother effects + // Remove the previous smoother summarization from the graph + BOOST_FOREACH(size_t slot, smootherSummarizationSlots_) { + removeFactor(slot); + } + smootherSummarizationSlots_.clear(); + +std::cout << "Removed Previous Smoother Summarization:" << std::endl; +for(size_t i = 0; i < graph_.size(); ++i) { + if(graph_[i]) { + std::cout << " f" << i << "( "; + BOOST_FOREACH(Key key, graph_[i]->keys()) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << ")" << std::endl; + } +} + +// TODO: Temporarily remove smoother effects + // Insert the updated smoother summarized factors + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, summarizedFactors) { + smootherSummarizationSlots_.push_back(insertFactor(factor)); + } + +std::cout << "Added New Smoother Summarization:" << std::endl; +for(size_t i = 0; i < graph_.size(); ++i) { + if(graph_[i]) { + std::cout << " f" << i << "( "; + BOOST_FOREACH(Key key, graph_[i]->keys()) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << ")" << std::endl; + } +} + +// TODO: Temporarily remove smoother effects + // Now that the smoother summarization has been updated, re-optimize the filter + update(); + + + // The filter should now be optimal (up to the linearization point of the separator variables) + // Calculate a new separator, a new filter summarization, a new smoother summarization, and the + // set of factors to send to the smoother + + + + // Force variables associated with root keys to keep the same linearization point + // TODO: This may be too many variables. It may only require that the smoother separator keys be kept constant. + gttic(enforce_consistency); + Values linpoint = theta_; + if(separatorValues_.size() > 0) { + linpoint.update(separatorValues_); + } + gttoc(enforce_consistency); + +std::cout << "ConcurrentBatchFilter::synchronize(*) Old Smoother Separator Keys: "; +BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + std::cout << DefaultKeyFormatter(key_value.key) << " "; +} +std::cout << std::endl; + +//std::cout << "ConcurrentBatchFilter::synchronize(*) Old Root Keys: "; +//BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, rootValues_) { +// std::cout << DefaultKeyFormatter(key_value.key) << " "; +//} +//std::cout << std::endl; + +//std::cout << "ConcurrentBatchFilter::synchronize(*) All Keys: "; +//BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, linpoint) { +// std::cout << DefaultKeyFormatter(key_value.key) << " "; +//} +//std::cout << std::endl; + + + // Calculate an ordering that puts variables newer than LAG at the top + gttic(compute_ordering); + std::set recentKeys = findKeysAfter(getCurrentTimestamp() - lag_); + std::map constraints; + BOOST_FOREACH(Key key, recentKeys) { + constraints[key] = 1; + } + Ordering ordering = *graph_.orderingCOLAMDConstrained(linpoint, constraints); + gttoc(compute_ordering); + +//typedef std::map Constraints; +//std::cout << "ConcurrentBatchFilter::synchronize(*) Constraints: "; +//BOOST_FOREACH(const Constraints::value_type key_group, constraints) { +// std::cout << DefaultKeyFormatter(key_group.first) << " "; +//} +//std::cout << std::endl; + +//ordering.print("ConcurrentBatchFilter::synchronize(*) Ordering:\n"); + + + // Create a Bayes Tree using iSAM2 cliques + gttic(create_bayes_tree); + JunctionTree jt(*graph_.linearize(linpoint, ordering)); + ISAM2Clique::shared_ptr root = jt.eliminate(parameters_.getEliminationFunction()); + BayesTree bayesTree; + bayesTree.insert(root); + gttoc(create_bayes_tree); + +//std::cout << "ConcurrentBatchFilter::synchronize(*) Create Bayes Tree:" << std::endl; +//SymbolicPrintTree(root, ordering, " "); + + // Find all of the filter cliques, and the smoother branches + // The smoother branches are defined as the children filter cliques that are not also filter cliques + gttic(identify_smoother_branches); + std::set filterCliques; + std::set smootherBranches; + BOOST_FOREACH(Key key, recentKeys) { + Index index = ordering.at(key); + const ISAM2Clique::shared_ptr& clique = bayesTree.nodes().at(index); + if(clique) { + // Add all of the child-cliques to the smoother branches + filterCliques.insert(clique); + smootherBranches.insert(clique->children().begin(), clique->children().end()); + } + } + BOOST_FOREACH(const ISAM2Clique::shared_ptr& filterClique, filterCliques) { + smootherBranches.erase(filterClique); + } + gttoc(identify_smoother_branches); + +std::cout << "ConcurrentBatchFilter::synchronize(*) Smoother Branches:" << std::endl; +BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) { + std::cout << " P( "; + BOOST_FOREACH(Index index, clique->conditional()->frontals()) { + std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; + } + if(clique->conditional()->nrParents() > 0) { std::cout << "| "; } + BOOST_FOREACH(Index index, clique->conditional()->parents()) { + std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; + } + std::cout << ")" << std::endl; +} + + // Find all of the frontal keys in the smoother branches + gttic(find_smoother_keys); + std::set smootherKeys; + std::queue smootherCliques; + BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) { + smootherCliques.push(clique); + } + while(smootherCliques.size() > 0) { + // Extract the frontal keys from the next clique + BOOST_FOREACH(Index index, smootherCliques.front()->conditional()->frontals()) { + smootherKeys.insert(ordering.key(index)); + } + // Add any children to the queue + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, smootherCliques.front()->children()) { + smootherCliques.push(child); + } + // Remove this clique from the queue + smootherCliques.pop(); + } + // Store the current linearization point for future transmission to the smoother (skip any variables in the previous root) + smootherValues_.clear(); + BOOST_FOREACH(Key key, smootherKeys) { + smootherValues_.insert(key, linpoint.at(key)); + } + gttoc(find_smoother_keys); + +std::cout << "ConcurrentBatchFilter::synchronize(*) Smoother Keys: "; +BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, smootherValues_) { + std::cout << DefaultKeyFormatter(key_value.key) << " "; +} +std::cout << std::endl; + + // Find the set of smoother separator keys + // The separator keys are defined as the set of parents to current branches + // + the parents of previous branches that are not new smoother keys + gttic(update_smoother_separator); + BOOST_FOREACH(Key key, smootherKeys) { + if(separatorValues_.exists(key)) { + separatorValues_.erase(key); + } + } + BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) { + BOOST_FOREACH(Index index, clique->conditional()->parents()) { + Key key = ordering.key(index); + if(!separatorValues_.exists(key)) + separatorValues_.insert(key, linpoint.at(key)); + } + } + gttoc(update_smoother_separator); + +std::cout << "ConcurrentBatchFilter::synchronize(*) New Smoother Separator Keys: "; +BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + std::cout << DefaultKeyFormatter(key_value.key) << " "; +} +std::cout << std::endl; + + // Calculate the new smoother summarization. + // This is potentially a combination of the previous summarization and cached factors from new smoother branches + gttic(extract_smoother_summarization); + // Add in previous summarization, marginalizing out non-smoother-separator keys + std::set smootherSeparatorKeys; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { + smootherSeparatorKeys.insert(key_value.key); + } + NonlinearFactorGraph smootherSummarization; + BOOST_FOREACH(size_t slot, smootherSummarizationSlots_) { +if(smootherBranches.size() == 0) { +graph_.at(slot)->print("Summarized Factor Before Marginalization:\n"); + NonlinearFactor::shared_ptr marginalFactor = marginalizeKeysFromFactor(graph_.at(slot), smootherSeparatorKeys); + if(marginalFactor) { + smootherSummarization.push_back(marginalFactor); + } +if(marginalFactor) { + marginalFactor->print("Summarized Factor After Marginalization:\n"); +} else { + std::cout << "Summarized Factor After Marginalization:\n{NULL}" << std::endl; +} +} + removeFactor(slot); + } + smootherSummarizationSlots_.clear(); + // Insert cached factors for any smoother branches + BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) { +clique->cachedFactor()->print("Summarized Cached Factor:\n"); + LinearizedGaussianFactor::shared_ptr factor; + if(const JacobianFactor::shared_ptr rhs = boost::dynamic_pointer_cast(clique->cachedFactor())) + factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(rhs, ordering, linpoint)); + else if(const HessianFactor::shared_ptr rhs = boost::dynamic_pointer_cast(clique->cachedFactor())) + factor = LinearizedHessianFactor::shared_ptr(new LinearizedHessianFactor(rhs, ordering, linpoint)); + else + throw std::invalid_argument("In ConcurrentBatchFilter::process(...), cached factor is neither a JacobianFactor nor a HessianFactor"); +factor->print("Summarized Linearized Factor:\n"); + smootherSummarization.push_back(factor); + } + gttoc(extract_smoother_summarization); + +std::cout << "ConcurrentBatchFilter::synchronize(*) Smoother Summarization:" << std::endl; +BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherSummarization) { + if(boost::dynamic_pointer_cast(factor)) + std::cout << " L( "; + else + std::cout << " f( "; + BOOST_FOREACH(Key key, factor->keys()) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << ")" << std::endl; +} + + // Find all of the factors that contain at least one smoother key + // These nonlinear factors will be sent to the smoother + gttic(find_smoother_factors); + std::set smootherFactorSlots = findFactorsWithAny(smootherKeys); + // Convert the set of slots into a set of factors + smootherFactors_.resize(0); + BOOST_FOREACH(size_t slot, smootherFactorSlots) { + smootherFactors_.push_back(graph_.at(slot)); + } + gttoc(find_smoother_factors); + +std::cout << "ConcurrentBatchFilter::synchronize(*) Smoother Factors:" << std::endl; +BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherFactors_) { + if(boost::dynamic_pointer_cast(factor)) + std::cout << " L( "; + else + std::cout << " f( "; + BOOST_FOREACH(Key key, factor->keys()) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << ")" << std::endl; +} + + // Find the Root Cliques + // The root is defined as the parents of the smoother separator + the path to the root + gttic(identify_root_cliques); + std::set rootCliques; + BOOST_FOREACH(Key key, smootherSeparatorKeys) { + Index index = ordering.at(key); + const ISAM2Clique::shared_ptr& clique = bayesTree.nodes().at(index); + if(clique) { + rootCliques.insert(clique); + } + } + BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, rootCliques) { + ISAM2Clique::shared_ptr rootClique = clique; + while(!rootClique->isRoot()) { + rootClique = rootClique->parent(); + rootCliques.insert(rootClique); + } + } + std::set rootKeys; + BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, rootCliques) { + BOOST_FOREACH(Index index, clique->conditional()->frontals()) { + rootKeys.insert(ordering.key(index)); + } + } + gttoc(identify_root_cliques); + +//std::cout << "ConcurrentBatchFilter::synchronize(*) Root Cliques:" << std::endl; +//BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, rootCliques) { +//std::cout << " P( "; +//BOOST_FOREACH(Index index, clique->conditional()->frontals()) { +// std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; +//} +//if(clique->conditional()->nrParents() > 0) { std::cout << "| "; } +//BOOST_FOREACH(Index index, clique->conditional()->parents()) { +// std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; +//} +//std::cout << ")" << std::endl; +//} + + // Update the keys and linearization points of the new root + gttic(update_root_values); + rootValues_.clear(); + BOOST_FOREACH(Key key, rootKeys) { + rootValues_.insert(key, linpoint.at(key)); + } + gttoc(update_root_values); + +//std::cout << "ConcurrentBatchFilter::synchronize(*) New Root Keys: "; +//BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, rootValues_) { +// std::cout << DefaultKeyFormatter(key_value.key) << " "; +//} +//std::cout << std::endl; + + // Identify the filter branches + // The filter branches are the children of the root that are not smoother branches. + gttic(identify_filter_branches); + std::set filterBranches; + // Find the children of the root + BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, rootCliques) { + filterBranches.insert(clique->children().begin(), clique->children().end()); + } + // Remove any root cliques that made it into the filter branches + BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, rootCliques) { + filterBranches.erase(clique); + } + // Remove the smoother branches + BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) { + filterBranches.erase(clique); + } + gttoc(identify_filter_branches); + +//std::cout << "ConcurrentBatchFilter::synchronize(*) Filter Branches:" << std::endl; +//BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, filterBranches) { +// std::cout << " P( "; +// BOOST_FOREACH(Index index, clique->conditional()->frontals()) { +// std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; +// } +// if(clique->conditional()->nrParents() > 0) { std::cout << "| "; } +// BOOST_FOREACH(Index index, clique->conditional()->parents()) { +// std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; +// } +// std::cout << ")" << std::endl; +//} + + // Extract cached factors from the filter branches and store for future transmission to the smoother + gttic(extract_filter_summarization); + filterSummarization_.resize(0); + BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, filterBranches) { + LinearizedGaussianFactor::shared_ptr factor; + if(const JacobianFactor::shared_ptr rhs = boost::dynamic_pointer_cast(clique->cachedFactor())) + factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(rhs, ordering, linpoint)); + else if(const HessianFactor::shared_ptr rhs = boost::dynamic_pointer_cast(clique->cachedFactor())) + factor = LinearizedHessianFactor::shared_ptr(new LinearizedHessianFactor(rhs, ordering, linpoint)); + else + throw std::invalid_argument("In ConcurrentBatchFilter::process(...), cached factor is neither a JacobianFactor nor a HessianFactor"); + filterSummarization_.push_back(factor); + } + gttoc(extract_filter_summarization); + + // Find all factors that contain only root clique variables + // This information is not contained in either the smoother + // summarization or the filter summarization. This must be sent + // to the smoother as well. Including it as part of the + // root/filter summarization + gttic(extract_root_summarization); + std::set rootFactorSlots = findFactorsWithOnly(rootKeys); + BOOST_FOREACH(size_t slot, rootFactorSlots) { + filterSummarization_.push_back(graph_.at(slot)); + } + gttoc(extract_root_summarization); + +//std::cout << "ConcurrentBatchFilter::synchronize(*) Filter Summarization:" << std::endl; +//BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, filterSummarization_) { +// if(boost::dynamic_pointer_cast(factor)) +// std::cout << " L( "; +// else +// std::cout << " f( "; +// BOOST_FOREACH(Key key, factor->keys()) { +// std::cout << DefaultKeyFormatter(key) << " "; +// } +// std::cout << ")" << std::endl; +//} + + gttic(purge_smoother_information); + // Remove nonlinear factors being sent to the smoother + BOOST_FOREACH(size_t slot, smootherFactorSlots) { + removeFactor(slot); + } + + // Remove the nonlinear keys being sent to the smoother + BOOST_FOREACH(Key key, smootherKeys) { + removeKey(key); + } + + // Add all of the smoother summarization factors to the filter, keeping track of their locations for later removal + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherSummarization) { + smootherSummarizationSlots_.push_back(insertFactor(factor)); + } + gttoc(purge_smoother_information); + + + + + +// // Check that solution before marginalization matches solution after marginalization +// Values solutionAfter; +// solutionAfter.insert(theta_); +// solutionAfter.update(smootherSeparatorValues_); +// Ordering orderingAfter = *graph_.orderingCOLAMD(solutionAfter); +// GaussianFactorGraph lingraphAfter = *graph_.linearize(solutionAfter, orderingAfter); +// GaussianMultifrontalSolver gmsAfter(lingraphAfter, true); +// VectorValues deltaAfter = *gmsAfter.optimize(); +// solutionAfter = solutionAfter.retract(deltaAfter, orderingAfter); +// +// BOOST_FOREACH(Key key, smootherKeys) { +// solutionBefore.erase(key); +// } +// +//std::cout << "Solution before marginalization matches solution after marginalization: " << solutionBefore.equals(solutionAfter, 1.0e-9) << std::endl; + +std::cout << "ConcurrentBatchFilter::synchronize(*) End" << std::endl; + + gttoc(synchronize); +} + +/* ************************************************************************* */ +void ConcurrentBatchFilter::getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& rootValues) { + + gttic(get_summarized_factors); + + // Copy the previous calculated smoother summarization factors into the output + summarizedFactors.push_back(filterSummarization_); + rootValues.insert(rootValues_); + + gttic(get_summarized_factors); +} + +/* ************************************************************************* */ +void ConcurrentBatchFilter::getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) { + + gttic(get_smoother_factors); + + // Copy the previous calculated smoother summarization factors into the output + smootherFactors.push_back(smootherFactors_); + smootherValues.insert(smootherValues_); + + gttic(get_smoother_factors); +} + +/* ************************************************************************* */ +void ConcurrentBatchFilter::postsync() { + + gttic(postsync); + + gttoc(postsync); +} + + +/* ************************************************************************* */ +size_t ConcurrentBatchFilter::insertFactor(const NonlinearFactor::shared_ptr& factor) { + + gttic(insert_factors); + + // Insert the factor into an existing hole in the factor graph, if possible + size_t slot; + if(availableSlots_.size() > 0) { + slot = availableSlots_.front(); + availableSlots_.pop(); + graph_.replace(slot, factor); + } else { + slot = graph_.size(); + graph_.push_back(factor); + } + // Update the FactorIndex + BOOST_FOREACH(Key key, *factor) { + factorIndex_[key].insert(slot); + } + + gttoc(insert_factors); + + return slot; +} + +/* ************************************************************************* */ +void ConcurrentBatchFilter::removeFactor(size_t slot) { + + gttic(remove_factors); + + // Remove references to this factor from the FactorIndex + BOOST_FOREACH(Key key, *(graph_.at(slot))) { + factorIndex_[key].erase(slot); + } + // Remove this factor from the graph + graph_.remove(slot); + // Mark the factor slot as avaiable + availableSlots_.push(slot); + + gttoc(remove_factors); +} + +/* ************************************************************************* */ +void ConcurrentBatchFilter::removeKey(Key key) { + + gttic(remove_keys); + + // Erase the key from the Timestamp->Key map + double timestamp = keyTimestampMap_.at(key); + + TimestampKeyMap::iterator iter = timestampKeyMap_.lower_bound(timestamp); + while(iter != timestampKeyMap_.end() && iter->first == timestamp) { + if(iter->second == key) { + timestampKeyMap_.erase(iter++); + } else { + ++iter; + } + } + // Erase the key from the Key->Timestamp map + keyTimestampMap_.erase(key); + + // Erase the key from the values + theta_.erase(key); + + // Erase the key from the set of linearized keys + // TODO: Should this ever even occur? + if(rootValues_.exists(key)) { + rootValues_.erase(key); + } + + gttoc(remove_keys); +} + +/* ************************************************************************* */ +void ConcurrentBatchFilter::updateKeyTimestampMap(const KeyTimestampMap& timestamps) { + + gttic(update_key_timestamp_map); + + // Loop through each key and add/update it in the map + BOOST_FOREACH(const KeyTimestampMap::value_type& key_timestamp, timestamps) { + // Check to see if this key already exists inthe database + KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first); + + // If the key already exists + if(keyIter != keyTimestampMap_.end()) { + // Find the entry in the Timestamp-Key database + std::pair range = timestampKeyMap_.equal_range(keyIter->second); + TimestampKeyMap::iterator timeIter = range.first; + while(timeIter->second != key_timestamp.first) { + ++timeIter; + } + // remove the entry in the Timestamp-Key database + timestampKeyMap_.erase(timeIter); + // insert an entry at the new time + timestampKeyMap_.insert(TimestampKeyMap::value_type(key_timestamp.second, key_timestamp.first)); + // update the Key-Timestamp database + keyIter->second = key_timestamp.second; + } else { + // Add the Key-Timestamp database + keyTimestampMap_.insert(key_timestamp); + // Add the key to the Timestamp-Key database + timestampKeyMap_.insert(TimestampKeyMap::value_type(key_timestamp.second, key_timestamp.first)); + } + } + + gttoc(update_key_timestamp_map); +} + +/* ************************************************************************* */ +double ConcurrentBatchFilter::getCurrentTimestamp() const { + + gttic(get_current_timestamp); + + double timestamp; + if(timestampKeyMap_.size() > 0) { + timestamp = timestampKeyMap_.rbegin()->first; + } else { + timestamp = -std::numeric_limits::max(); + } + + gttoc(get_current_timestamp); + + return timestamp; +} + +/* ************************************************************************* */ +std::set ConcurrentBatchFilter::findKeysBefore(double timestamp) const { + + gttic(find_keys_before); + + std::set keys; + TimestampKeyMap::const_iterator end = timestampKeyMap_.lower_bound(timestamp); + for(TimestampKeyMap::const_iterator iter = timestampKeyMap_.begin(); iter != end; ++iter) { + keys.insert(iter->second); + } + + gttoc(find_keys_before); + + return keys; +} + +/* ************************************************************************* */ +std::set ConcurrentBatchFilter::findKeysAfter(double timestamp) const { + + gttic(find_keys_after); + + std::set keys; + TimestampKeyMap::const_iterator begin = timestampKeyMap_.lower_bound(timestamp); + for(TimestampKeyMap::const_iterator iter = begin; iter != timestampKeyMap_.end(); ++iter) { + keys.insert(iter->second); + } + + gttoc(find_keys_after); + + return keys; +} + +/* ************************************************************************* */ +std::set ConcurrentBatchFilter::findFactorsWithAny(const std::set& keys) const { + // Find the set of factor slots for each specified key + std::set factorSlots; + BOOST_FOREACH(Key key, keys) { + FactorIndex::const_iterator iter = factorIndex_.find(key); + if(iter != factorIndex_.end()) { + factorSlots.insert(iter->second.begin(), iter->second.end()); + } + } + + return factorSlots; +} + +/* ************************************************************************* */ +std::set ConcurrentBatchFilter::findFactorsWithOnly(const std::set& keys) const { + // Find the set of factor slots with any of the provided keys + std::set factorSlots = findFactorsWithAny(keys); + // Test each factor for non-specified keys + std::set::iterator slot = factorSlots.begin(); + while(slot != factorSlots.end()) { + const NonlinearFactor::shared_ptr& factor = graph_.at(*slot); + std::set factorKeys(factor->begin(), factor->end()); // ensure the keys are sorted + if(!std::includes(keys.begin(), keys.end(), factorKeys.begin(), factorKeys.end())) { + factorSlots.erase(slot++); + } else { + ++slot; + } + } + + return factorSlots; +} + +/* ************************************************************************* */ +NonlinearFactor::shared_ptr ConcurrentBatchFilter::marginalizeKeysFromFactor(const NonlinearFactor::shared_ptr& factor, const std::set& remainingKeys) const { + + const bool debug = false; + if(debug) std::cout << "ConcurrentBatchFilter::marginalizeKeys() START" << std::endl; + + LinearizedGaussianFactor::shared_ptr linearizedFactor = boost::dynamic_pointer_cast(factor); + assert(linearizedFactor); + + // Sort the keys for this factor + std::set factorKeys; + BOOST_FOREACH(Key key, *linearizedFactor) { + factorKeys.insert(key); + } + + // Calculate the set of keys to marginalize + std::set marginalizeKeys; + std::set_difference(factorKeys.begin(), factorKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(marginalizeKeys, marginalizeKeys.end())); + + // + if(marginalizeKeys.size() == 0) { + // No keys need to be marginalized out. Simply return the original factor. + return linearizedFactor; + } else if(marginalizeKeys.size() == linearizedFactor->size()) { + // All keys need to be marginalized out. Return an empty factor + return NonlinearFactor::shared_ptr(); + } else { + // (0) Create an ordering with the remaining keys last + Ordering ordering; + BOOST_FOREACH(Key key, marginalizeKeys) { + ordering.push_back(key); + } + BOOST_FOREACH(Key key, remainingKeys) { + ordering.push_back(key); + } + + // (1) construct a linear factor graph + GaussianFactorGraph graph; + graph.push_back( linearizedFactor->linearize(linearizedFactor->linearizationPoint(), ordering) ); + + // (2) solve for the marginal factor + // Perform partial elimination, resulting in a conditional probability ( P(MarginalizedVariable | RemainingVariables) + // and factors on the remaining variables ( f(RemainingVariables) ). These are the factors we need to add to iSAM2 + std::vector variables; + BOOST_FOREACH(Key key, marginalizeKeys) { + variables.push_back(ordering.at(key)); + } + std::pair result = graph.eliminate(variables); + graph = result.second; + + // (3) convert the marginal factors into Linearized Factors + NonlinearFactor::shared_ptr marginalFactor; + assert(graph.size() <= 1); + if(graph.size() > 0) { + // These factors are all generated from BayesNet conditionals. They should all be Jacobians. + JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast(graph.at(0)); + assert(jacobianFactor); + marginalFactor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(jacobianFactor, ordering, linearizedFactor->linearizationPoint())); + } + return marginalFactor; + } +} + +/* ************************************************************************* */ + +}/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h new file mode 100644 index 000000000..2a0f1a08b --- /dev/null +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -0,0 +1,195 @@ +/* ---------------------------------------------------------------------------- + + * 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 ConcurrentBatchFilter.h + * @brief A Levenberg-Marquardt Batch Filter that implements the + * Concurrent Filtering and Smoothing interface. + * @author Stephen Williams + */ + +// \callgraph +#pragma once + +#include "ConcurrentFilteringAndSmoothing.h" +#include +#include +#include +#include + +namespace gtsam { + +/** + * A Levenberg-Marquardt Batch Filter that implements the Concurrent Filtering and Smoother interface. + */ +class ConcurrentBatchFilter : public ConcurrentFilter { + +public: + + typedef ConcurrentFilter Base; ///< typedef for base class + typedef std::map KeyTimestampMap; ///< Typedef for a Key-Timestamp map/database + typedef std::multimap TimestampKeyMap;///< Typedef for a Timestamp-Key map/database + + /** + * Meta information returned about the update + */ + // TODO: Think of some more things to put here + struct Result { + size_t iterations; ///< The number of optimizer iterations performed + size_t nonlinearVariables; ///< The number of variables that can be relinearized + size_t linearVariables; ///< The number of variables that must keep a constant linearization point + double error; ///< The final factor graph error + Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {}; + }; + + /** Default constructor */ + ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters, double lag) : parameters_(parameters), lag_(lag) {}; + + /** Default destructor */ + virtual ~ConcurrentBatchFilter() {}; + + // Implement a GTSAM standard 'print' function + void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** Compute the current best estimate of all variables and return a full Values structure. + * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&). + */ + Values calculateEstimate() const { + return theta_; + } + + /** Compute the current best estimate of a single variable. This is generally faster than + * calling the no-argument version of calculateEstimate if only specific variables are needed. + * @param key + * @return + */ + template + VALUE calculateEstimate(const Key key) const { + return theta_.at(key); + } + + /** + * Add new factors and variables to the filter. + * + * Add new measurements, and optionally new variables, to the filter. + * This runs a full update step of the derived filter algorithm + * + * @param newFactors The new factors to be added to the smoother + * @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. + */ + Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), + const KeyTimestampMap& timestamps = KeyTimestampMap()); + +protected: + + /** A typedef defining an Key-Factor mapping **/ + typedef std::map > FactorIndex; + + LevenbergMarquardtParams parameters_; ///< LM parameters + double lag_; ///< Time before keys are transitioned from the filter to the smoother + NonlinearFactorGraph graph_; ///< The graph of all the smoother factors + Values theta_; ///< Current solution + TimestampKeyMap timestampKeyMap_; ///< The set of keys associated with each timestamp + KeyTimestampMap keyTimestampMap_; ///< The timestamp associated with each key + std::queue availableSlots_; ///< The set of available factor graph slots caused by deleting factors + FactorIndex factorIndex_; ///< A cross-reference structure to allow efficient factor lookups by key + + std::vector smootherSummarizationSlots_; ///< The slots in graph for the last set of smoother summarized factors + Values separatorValues_; + + // 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 + Values rootValues_; ///< The set of keys to be kept in the root and their linearization points + NonlinearFactorGraph smootherFactors_; ///< A temporary holding place for the set of full nonlinear factors being sent to the smoother + Values smootherValues_; ///< A temporary holding place for the linearization points of all keys being sent to the smoother + + /** + * Perform any required operations before the synchronization process starts. + * Called by 'synchronize' + */ + virtual void presync(); + + /** + * Populate the provided containers with factors that constitute the filter branch summarization + * needed by the smoother. Also, linearization points for the new root clique must be provided. + * + * @param summarizedFactors The summarized factors for the filter branch + * @param rootValues The linearization points of the root clique variables + */ + virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& rootValues); + + /** + * Populate the provided containers with factors being sent to the smoother from the filter. These + * may be original nonlinear factors, or factors encoding a summarization of the filter information. + * The specifics will be implementation-specific for a given filter. + * + * @param smootherFactors The new factors to be added to the smoother + * @param smootherValues The linearization points of any new variables + */ + virtual void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues); + + /** + * Apply the updated version of the smoother branch summarized factors. + * + * @param summarizedFactors An updated version of the smoother branch summarized factors + */ + virtual void synchronize(const NonlinearFactorGraph& summarizedFactors); + + /** + * Perform any required operations after the synchronization process finishes. + * Called by 'synchronize' + */ + virtual void postsync(); + + + /** Augment the graph with a new factor + * + * @param factor The new factor to add to the graph + * @return The slot in the graph where the factor was inserted + */ + size_t insertFactor(const NonlinearFactor::shared_ptr& factor); + + /** Remove a factor from the graph by slot index */ + void removeFactor(size_t slot); + + /** Remove the specified key from all data structures */ + void removeKey(Key key); + + /** Update the Timestamps associated with the keys */ + void updateKeyTimestampMap(const KeyTimestampMap& newTimestamps); + + /** Find the most recent timestamp of the system */ + double getCurrentTimestamp() const; + + /** Find all of the keys associated with timestamps before the provided time */ + std::set findKeysBefore(double timestamp) const; + + /** Find all of the keys associated with timestamps after the provided time */ + std::set findKeysAfter(double timestamp) const; + + /** Find all of the nonlinear factors that contain any of the provided keys */ + std::set findFactorsWithAny(const std::set& keys) const; + + /** Find all of the nonlinear factors that contain only the provided keys */ + std::set findFactorsWithOnly(const std::set& keys) const; + + /** Create linearized factors from any factors remaining after marginalizing out the requested keys */ + NonlinearFactor::shared_ptr marginalizeKeysFromFactor(const NonlinearFactor::shared_ptr& factor, const std::set& remainingKeys) const; + +private: + typedef BayesTree::sharedClique Clique; + static void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = ""); + +}; // ConcurrentBatchFilter + +}/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp new file mode 100644 index 000000000..d00b6d192 --- /dev/null +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -0,0 +1,470 @@ +/* ---------------------------------------------------------------------------- + + * 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 ConcurrentBatchSmoother.cpp + * @brief A Levenberg-Marquardt Batch Smoother that implements the + * Concurrent Filtering and Smoothing interface. + * @author Stephen Williams + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +void ConcurrentBatchSmoother::SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent) { + std::cout << indent << "P( "; + BOOST_FOREACH(Index index, clique->conditional()->frontals()){ + std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; + } + if(clique->conditional()->nrParents() > 0) { + std::cout << "| "; + } + BOOST_FOREACH(Index index, clique->conditional()->parents()){ + std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; + } + std::cout << ")" << std::endl; + + BOOST_FOREACH(const Clique& child, clique->children()) { + SymbolicPrintTree(child, ordering, indent+" "); + } +} + +/* ************************************************************************* */ +void ConcurrentBatchSmoother::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::cout << s; + graph_.print("Factors:\n"); + theta_.print("Values:\n"); +} + +/* ************************************************************************* */ +ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta) { + + gttic(update); + + // Create result structure + Result result; + + gttic(augment_system); + // Add the new factors to the graph + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { + insertFactor(factor); + } + // Add the new variables to theta + theta_.insert(newTheta); + gttoc(augment_system); + + // Optimize the graph, updating theta + gttic(optimize); + if(graph_.size() > 0){ + // Create an L-M optimizer + Values linpoint; + linpoint.insert(theta_); + if(rootValues_.size() > 0) { + linpoint.insert(rootValues_); + } + LevenbergMarquardtOptimizer optimizer(graph_, linpoint, parameters_); + + // Use a custom optimization loop so the linearization points can be controlled + double currentError; + do { + // Do next iteration + gttic(optimizer_iteration); + currentError = optimizer.error(); + optimizer.iterate(); + gttoc(optimizer_iteration); + + // Force variables associated with root keys to keep the same linearization point + gttic(enforce_consistency); + if(rootValues_.size() > 0) { + // Put the old values of the root keys back into the optimizer state + optimizer.state().values.update(rootValues_); + optimizer.state().error = graph_.error(optimizer.state().values); + } + gttoc(enforce_consistency); + + // Maybe show output + if(parameters_.verbosity >= NonlinearOptimizerParams::VALUES) optimizer.values().print("newValues"); + if(parameters_.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "newError: " << optimizer.error() << std::endl; + } while(optimizer.iterations() < parameters_.maxIterations && + !checkConvergence(parameters_.relativeErrorTol, parameters_.absoluteErrorTol, + parameters_.errorTol, currentError, optimizer.error(), parameters_.verbosity)); + + // Update theta from the optimizer, then remove root variables + theta_ = optimizer.values(); + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, rootValues_) { + theta_.erase(key_value.key); + } + + result.iterations = optimizer.state().iterations; + result.nonlinearVariables = theta_.size(); + result.linearVariables = rootValues_.size(); + result.error = optimizer.state().error; + } + gttoc(optimize); + + // Move all of the Pre-Sync code to the end of the update. This allows the smoother to perform these + // calculations while the filter is still running + gttic(presync); + // Calculate and store the information passed up to the root clique. This requires: + // 1) Calculate an ordering that forces the rootKey variables to be in the root + // 2) Perform an elimination, constructing a Bayes Tree from the currnet + // variable values. This elimination will use the iSAM2 version of a clique so + // that cached factors are stored + // 3) Verify the root's cached factors involve only root keys; all others should + // be marginalized + // 4) Convert cached factors into 'Linearized' nonlinear factors + + if(rootValues_.size() > 0) { + // Force variables associated with root keys to keep the same linearization point + gttic(enforce_consistency); + Values linpoint; + linpoint.insert(theta_); + linpoint.insert(rootValues_); + +//linpoint.print("ConcurrentBatchSmoother::presync() LinPoint:\n"); + + gttoc(enforce_consistency); + + // Calculate a root-constrained ordering + gttic(compute_ordering); + std::map constraints; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, rootValues_) { + constraints[key_value.key] = 1; + } + Ordering ordering = *graph_.orderingCOLAMDConstrained(linpoint, constraints); + gttoc(compute_ordering); + + // Create a Bayes Tree using iSAM2 cliques + gttic(create_bayes_tree); + JunctionTree jt(*graph_.linearize(linpoint, ordering)); + ISAM2Clique::shared_ptr root = jt.eliminate(parameters_.getEliminationFunction()); + BayesTree bayesTree; + bayesTree.insert(root); + gttoc(create_bayes_tree); + +//ordering.print("ConcurrentBatchSmoother::presync() Ordering:\n"); +std::cout << "ConcurrentBatchSmoother::presync() Root Keys: "; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, rootValues_) { std::cout << DefaultKeyFormatter(key_value.key) << " "; } std::cout << std::endl; +std::cout << "ConcurrentBatchSmoother::presync() Bayes Tree:" << std::endl; +SymbolicPrintTree(root, ordering, " "); + + // Extract the marginal factors from the smoother + // For any non-filter factor that involves a root variable, + // calculate its marginal on the root variables using the + // current linearization point. + + // Find all of the smoother branches as the children of root cliques that are not also root cliques + gttic(find_smoother_branches); + std::set rootCliques; + std::set smootherBranches; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, rootValues_) { + const ISAM2Clique::shared_ptr& clique = bayesTree.nodes().at(ordering.at(key_value.key)); + if(clique) { + rootCliques.insert(clique); + smootherBranches.insert(clique->children().begin(), clique->children().end()); + } + } + BOOST_FOREACH(const ISAM2Clique::shared_ptr& rootClique, rootCliques) { + smootherBranches.erase(rootClique); + } + gttoc(find_smoother_branches); + + // Extract the cached factors on the root cliques from the smoother branches + gttic(extract_cached_factors); + GaussianFactorGraph cachedFactors; + BOOST_FOREACH(const ISAM2Clique::shared_ptr& clique, smootherBranches) { + cachedFactors.push_back(clique->cachedFactor()); + } + gttoc(extract_cached_factors); + +std::cout << "ConcurrentBatchSmoother::presync() Cached Factors Before:" << std::endl; +BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, cachedFactors) { + std::cout << " g( "; + BOOST_FOREACH(Index index, factor->keys()) { + std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; + } + std::cout << ")" << std::endl; +} + + // Marginalize out any additional (non-root) variables + gttic(marginalize_extra_variables); + // The rootKeys have been ordered last, so their linear indices will be { linpoint.size()-rootKeys.size() :: linpoint.size()-1 } + Index minRootIndex = linpoint.size() - rootValues_.size(); + // Calculate the set of keys to be marginalized + FastSet cachedIndices = cachedFactors.keys(); + std::vector marginalizeIndices; + std::remove_copy_if(cachedIndices.begin(), cachedIndices.end(), std::back_inserter(marginalizeIndices), boost::lambda::_1 >= minRootIndex); + +std::cout << "ConcurrentBatchSmoother::presync() Marginalize Keys: "; +BOOST_FOREACH(Index index, marginalizeIndices) { std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; } +std::cout << std::endl; + + // If non-root-keys are present, marginalize them out + if(marginalizeIndices.size() > 0) { + // Eliminate the extra variables, stored the remaining factors back into the 'cachedFactors' graph + GaussianConditional::shared_ptr conditional; + boost::tie(conditional, cachedFactors) = cachedFactors.eliminate(marginalizeIndices, parameters_.getEliminationFunction()); + } + gttoc(marginalize_extra_variables); + +std::cout << "ConcurrentBatchSmoother::presync() Cached Factors After:" << std::endl; +BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, cachedFactors) { + std::cout << " g( "; + BOOST_FOREACH(Index index, factor->keys()) { + std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; + } + std::cout << ")" << std::endl; +} + + // Convert factors into 'Linearized' nonlinear factors + gttic(store_cached_factors); + smootherSummarization_.resize(0); + BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, cachedFactors) { + LinearizedGaussianFactor::shared_ptr factor; + if(const JacobianFactor::shared_ptr rhs = boost::dynamic_pointer_cast(gaussianFactor)) + factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(rhs, ordering, linpoint)); + else if(const HessianFactor::shared_ptr rhs = boost::dynamic_pointer_cast(gaussianFactor)) + factor = LinearizedHessianFactor::shared_ptr(new LinearizedHessianFactor(rhs, ordering, linpoint)); + else + throw std::invalid_argument("In ConcurrentBatchSmoother::presync(...), cached factor is neither a JacobianFactor nor a HessianFactor"); + smootherSummarization_.push_back(factor); + } + gttoc(store_cached_factors); + +std::cout << "ConcurrentBatchSmoother::presync() Smoother Summarization:" << std::endl; +BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherSummarization_) { + std::cout << " f( "; + BOOST_FOREACH(Key key, factor->keys()) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << ")" << std::endl; +} + + } + gttoc(presync); + + gttoc(update); + + return result; +} + +/* ************************************************************************* */ +void ConcurrentBatchSmoother::presync() { + + gttic(presync); + + + gttoc(presync); +} + +/* ************************************************************************* */ +void ConcurrentBatchSmoother::getSummarizedFactors(NonlinearFactorGraph& summarizedFactors) { + + gttic(get_summarized_factors); + + // Copy the previous calculated smoother summarization factors into the output + summarizedFactors.push_back(smootherSummarization_); + + gttic(get_summarized_factors); +} + +/* ************************************************************************* */ +void ConcurrentBatchSmoother::synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, + const NonlinearFactorGraph& summarizedFactors, const Values& rootValues) { + + gttic(synchronize); + + // Remove the previous filter summarization from the graph + BOOST_FOREACH(size_t slot, filterSummarizationSlots_) { + removeFactor(slot); + } + filterSummarizationSlots_.clear(); + + // Insert the new filter summarized factors + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, summarizedFactors) { + filterSummarizationSlots_.push_back(insertFactor(factor)); + } + + // Insert the new smoother factors + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, smootherFactors) { + insertFactor(factor); + } + + // Insert new linpoints into the values + theta_.insert(smootherValues); + + // Update the list of root keys + rootValues_ = rootValues; + + gttoc(synchronize); +} + +/* ************************************************************************* */ +void ConcurrentBatchSmoother::postsync() { + + gttic(postsync); + + gttoc(postsync); +} + +/* ************************************************************************* */ +size_t ConcurrentBatchSmoother::insertFactor(const NonlinearFactor::shared_ptr& factor) { + + gttic(insert_factor); + + // Insert the factor into an existing hole in the factor graph, if possible + size_t slot; + if(availableSlots_.size() > 0) { + slot = availableSlots_.front(); + availableSlots_.pop(); + graph_.replace(slot, factor); + } else { + slot = graph_.size(); + graph_.push_back(factor); + } + // Update the FactorIndex + BOOST_FOREACH(Key key, *factor) { + factorIndex_[key].insert(slot); + } + + gttoc(insert_factors); + + return slot; +} + +/* ************************************************************************* */ +void ConcurrentBatchSmoother::removeFactor(size_t slot) { + + gttic(remove_factors); + + // Remove references to this factor from the FactorIndex + BOOST_FOREACH(Key key, *(graph_.at(slot))) { + factorIndex_[key].erase(slot); + } + // Remove this factor from the graph + graph_.remove(slot); + // Mark the factor slot as avaiable + availableSlots_.push(slot); + + gttoc(remove_factors); +} + +/* ************************************************************************* */ +std::set ConcurrentBatchSmoother::findFactorsWithAny(const std::set& keys) const { + // Find the set of factor slots for each specified key + std::set factorSlots; + BOOST_FOREACH(Key key, keys) { + FactorIndex::const_iterator iter = factorIndex_.find(key); + if(iter != factorIndex_.end()) { + factorSlots.insert(iter->second.begin(), iter->second.end()); + } + } + + return factorSlots; +} + +/* ************************************************************************* */ +std::set ConcurrentBatchSmoother::findFactorsWithOnly(const std::set& keys) const { + // Find the set of factor slots with any of the provided keys + std::set factorSlots = findFactorsWithAny(keys); + // Test each factor for non-specified keys + std::set::iterator slot = factorSlots.begin(); + while(slot != factorSlots.end()) { + const NonlinearFactor::shared_ptr& factor = graph_.at(*slot); + std::set factorKeys(factor->begin(), factor->end()); // ensure the keys are sorted + if(!std::includes(keys.begin(), keys.end(), factorKeys.begin(), factorKeys.end())) { + factorSlots.erase(slot++); + } else { + ++slot; + } + } + + return factorSlots; +} + +/* ************************************************************************* */ +NonlinearFactor::shared_ptr ConcurrentBatchSmoother::marginalizeKeysFromFactor(const NonlinearFactor::shared_ptr& factor, const std::set& keysToKeep, const Values& theta) const { + +factor->print("Factor Before:\n"); + + // Sort the keys for this factor + std::set factorKeys; + BOOST_FOREACH(Key key, *factor) { + factorKeys.insert(key); + } + + // Calculate the set of keys to marginalize + std::set marginalizeKeys; + std::set_difference(factorKeys.begin(), factorKeys.end(), keysToKeep.begin(), keysToKeep.end(), std::inserter(marginalizeKeys, marginalizeKeys.end())); + std::set remainingKeys; + std::set_intersection(factorKeys.begin(), factorKeys.end(), keysToKeep.begin(), keysToKeep.end(), std::inserter(remainingKeys, remainingKeys.end())); + + // + if(marginalizeKeys.size() == 0) { + // No keys need to be marginalized out. Simply return the original factor. + return factor; + } else if(marginalizeKeys.size() == factor->size()) { + // All keys need to be marginalized out. Return an empty factor + return NonlinearFactor::shared_ptr(); + } else { + // (0) Create an ordering with the remaining keys last + Ordering ordering; + BOOST_FOREACH(Key key, marginalizeKeys) { + ordering.push_back(key); + } + BOOST_FOREACH(Key key, remainingKeys) { + ordering.push_back(key); + } +ordering.print("Ordering:\n"); + + // (1) construct a linear factor graph + GaussianFactorGraph graph; + graph.push_back( factor->linearize(theta, ordering) ); +graph.at(0)->print("Linear Factor Before:\n"); + + // (2) solve for the marginal factor + // Perform partial elimination, resulting in a conditional probability ( P(MarginalizedVariable | RemainingVariables) + // and factors on the remaining variables ( f(RemainingVariables) ). These are the factors we need to add to iSAM2 + std::vector variables; + BOOST_FOREACH(Key key, marginalizeKeys) { + variables.push_back(ordering.at(key)); + } +// std::pair result = graph.eliminate(variables); + GaussianFactorGraph::EliminationResult result = EliminateQR(graph, marginalizeKeys.size()); +result.first->print("Resulting Conditional:\n"); +result.second->print("Resulting Linear Factor:\n"); +// graph = result.second; + graph.replace(0, result.second); + + // (3) convert the marginal factors into Linearized Factors + NonlinearFactor::shared_ptr marginalFactor; + assert(graph.size() <= 1); + if(graph.size() > 0) { +graph.at(0)->print("Linear Factor After:\n"); + // These factors are all generated from BayesNet conditionals. They should all be Jacobians. + JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast(graph.at(0)); + assert(jacobianFactor); + marginalFactor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(jacobianFactor, ordering, theta)); + } +marginalFactor->print("Factor After:\n"); + return marginalFactor; + } +} + +/* ************************************************************************* */ + +}/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h new file mode 100644 index 000000000..1e1419c97 --- /dev/null +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -0,0 +1,164 @@ +/* ---------------------------------------------------------------------------- + + * 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 ConcurrentBatchSmoother.h + * @brief A Levenberg-Marquardt Batch Smoother that implements the + * Concurrent Filtering and Smoothing interface. + * @author Stephen Williams + */ + +// \callgraph +#pragma once + +#include "ConcurrentFilteringAndSmoothing.h" +#include +#include +#include + +namespace gtsam { + +/** + * A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoother interface. + */ +class ConcurrentBatchSmoother : public ConcurrentSmoother { + +public: + + typedef ConcurrentSmoother Base; ///< typedef for base class + + /** + * Meta information returned about the update + */ + // TODO: Think of some more things to put here + struct Result { + size_t iterations; ///< The number of optimizer iterations performed + size_t nonlinearVariables; ///< The number of variables that can be relinearized + size_t linearVariables; ///< The number of variables that must keep a constant linearization point + double error; ///< The final factor graph error + Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {}; + }; + + /** Default constructor */ + ConcurrentBatchSmoother(const LevenbergMarquardtParams& parameters) : parameters_(parameters) {}; + + /** Default destructor */ + virtual ~ConcurrentBatchSmoother() {}; + + // Implement a GTSAM standard 'print' function + void print(const std::string& s = "Concurrent Batch Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** Compute the current best estimate of all variables and return a full Values structure. + * If only a single variable is needed, it may be faster to call calculateEstimate(const KEY&). + */ + Values calculateEstimate() const { + return theta_; + } + + /** Compute the current best estimate of a single variable. This is generally faster than + * calling the no-argument version of calculateEstimate if only specific variables are needed. + * @param key + * @return + */ + template + VALUE calculateEstimate(const Key key) const { + return theta_.at(key); + } + + /** + * Add new factors and variables to the smoother. + * + * Add new measurements, and optionally new variables, to the smoother. + * This runs a full step of the ISAM2 algorithm, relinearizing and updating + * the solution as needed, according to the wildfire and relinearize + * thresholds. + * + * @param newFactors The new factors to be added to the smoother + * @param newTheta Initialization points for new variables to be added to the smoother + * You must include here all new variables occuring in newFactors (which were not already + * in the smoother). There must not be any variables here that do not occur in newFactors, + * and additionally, variables that were already in the system must not be included here. + */ + Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values()); + + +protected: + + /** A typedef defining an Key-Factor mapping **/ + typedef std::map > FactorIndex; + + LevenbergMarquardtParams parameters_; ///< LM parameters + NonlinearFactorGraph graph_; ///< The graph of all the smoother factors + Values theta_; ///< Current solution + Values rootValues_; ///< The set of keys to be kept in the root and their linearization points + std::queue availableSlots_; ///< The set of available factor graph slots caused by deleting factors + FactorIndex factorIndex_; ///< A cross-reference structure to allow efficient factor lookups by key + std::vector filterSummarizationSlots_; ///< The slots in graph for the last set of filter summarized factors + NonlinearFactorGraph smootherSummarization_; ///< A temporary holding place for calculated smoother summarization + + /** + * Perform any required operations before the synchronization process starts. + * Called by 'synchronize' + */ + virtual void presync(); + + /** + * Populate the provided containers with factors that constitute the smoother branch summarization + * needed by the filter. + * + * @param summarizedFactors The summarized factors for the filter branch + */ + virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors); + + /** + * Apply the new smoother factors sent by the filter, and the updated version of the filter + * branch summarized factors. + * + * @param smootherFactors A set of new factors added to the smoother from the filter + * @param smootherValues Linearization points for any new variables + * @param summarizedFactors An updated version of the filter branch summarized factors + * @param rootValues The linearization point of the root variables + */ + virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, + const NonlinearFactorGraph& summarizedFactors, const Values& rootValues); + + /** + * Perform any required operations after the synchronization process finishes. + * Called by 'synchronize' + */ + virtual void postsync(); + + /** Augment the graph with a new factor + * + * @param factors The factor to add to the graph + * @return The slot in the graph it was inserted into + */ + size_t insertFactor(const NonlinearFactor::shared_ptr& factor); + + /** Remove a factor from the graph by slot index */ + void removeFactor(size_t slot); + + /** Find all of the nonlinear factors that contain any of the provided keys */ + std::set findFactorsWithAny(const std::set& keys) const; + + /** Find all of the nonlinear factors that contain only the provided keys */ + std::set findFactorsWithOnly(const std::set& keys) const; + + /** Create a linearized factor from the information remaining after marginalizing out the requested keys */ + NonlinearFactor::shared_ptr marginalizeKeysFromFactor(const NonlinearFactor::shared_ptr& factor, const std::set& keysToKeep, const Values& theta) const; + +private: + typedef BayesTree::sharedClique Clique; + static void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = ""); + +}; // ConcurrentBatchSmoother + +}/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp new file mode 100644 index 000000000..52ce1cd9b --- /dev/null +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp @@ -0,0 +1,49 @@ +/* ---------------------------------------------------------------------------- + + * 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 ConcurrentFilteringAndSmoothing.cpp + * @brief Base classes for the 'filter' and 'smoother' portion of the Concurrent + * Filtering and Smoothing architecture, as well as an external synchronization + * function. These classes act as an interface only. + * @author Stephen Williams + */ + +// \callgraph + +#include "ConcurrentFilteringAndSmoothing.h" + +namespace gtsam { + +void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother) { + + NonlinearFactorGraph smootherFactors, filterSumarization, smootherSummarization; + Values smootherValues, rootValues; + + // Call the pre-sync functions of the filter and smoother + filter.presync(); + smoother.presync(); + + // Get the updates from the smoother and apply them to the filter + smoother.getSummarizedFactors(smootherSummarization); + filter.synchronize(smootherSummarization); + + // Get the updates from the filter and apply them to the smoother + filter.getSmootherFactors(smootherFactors, smootherValues); + filter.getSummarizedFactors(filterSumarization, rootValues); + smoother.synchronize(smootherFactors, smootherValues, filterSumarization, rootValues); + + // Call the post-sync functions of the filter and smoother + filter.postsync(); + smoother.postsync(); +} + +}/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h new file mode 100644 index 000000000..b91391b98 --- /dev/null +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h @@ -0,0 +1,137 @@ +/* ---------------------------------------------------------------------------- + + * 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 ConcurrentFilteringAndSmoothing.h + * @brief Base classes for the 'filter' and 'smoother' portion of the Concurrent + * Filtering and Smoothing architecture, as well as an external synchronization + * function. These classes act as an interface only. + * @author Stephen Williams + */ + +// \callgraph +#pragma once + +#include + +namespace gtsam { + +// Forward declare the Filter and Smoother classes for the 'synchronize' function +class ConcurrentFilter; +class ConcurrentSmoother; + +void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother); + +/** + * The interface for the 'Filter' portion of the Concurrent Filtering and Smoother architecture. + */ +class ConcurrentFilter { + +protected: + + friend void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother); + + /** Default constructor */ + ConcurrentFilter() {}; + + /** Default destructor */ + virtual ~ConcurrentFilter() {}; + + /** + * Perform any required operations before the synchronization process starts. + * Called by 'synchronize' + */ + virtual void presync() {}; + + /** + * Populate the provided containers with factors that constitute the filter branch summarization + * needed by the smoother. Also, linearization points for the new root clique must be provided. + * + * @param summarizedFactors The summarized factors for the filter branch + * @param rootValues The linearization points of the root clique variables + */ + virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& rootValues) = 0; + + /** + * Populate the provided containers with factors being sent to the smoother from the filter. These + * may be original nonlinear factors, or factors encoding a summarization of the filter information. + * The specifics will be implementation-specific for a given filter. + * + * @param smootherFactors The new factors to be added to the smoother + * @param smootherValues The linearization points of any new variables + */ + virtual void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) = 0; + + /** + * Apply the updated version of the smoother branch summarized factors. + * + * @param summarizedFactors An updated version of the smoother branch summarized factors + */ + virtual void synchronize(const NonlinearFactorGraph& summarizedFactors) = 0; + + /** + * Perform any required operations after the synchronization process finishes. + * Called by 'synchronize' + */ + virtual void postsync() {}; + +}; // ConcurrentFilter + +/** + * The interface for the 'Smoother' portion of the Concurrent Filtering and Smoother architecture. + */ +class ConcurrentSmoother { + +protected: + + friend void synchronize(ConcurrentFilter& filter, ConcurrentSmoother& smoother); + + /** Default constructor */ + ConcurrentSmoother() {}; + + /** Default destructor */ + virtual ~ConcurrentSmoother() {}; + + /** + * Perform any required operations before the synchronization process starts. + * Called by 'synchronize' + */ + virtual void presync() {}; + + /** + * Populate the provided containers with factors that constitute the smoother branch summarization + * needed by the filter. + * + * @param summarizedFactors The summarized factors for the filter branch + */ + virtual void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors) = 0; + + /** + * Apply the new smoother factors sent by the filter, and the updated version of the filter + * branch summarized factors. + * + * @param smootherFactors A set of new factors added to the smoother from the filter + * @param smootherValues Linearization points for any new variables + * @param summarizedFactors An updated version of the filter branch summarized factors + * @param rootValues The linearization point of the root variables + */ + virtual void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, + const NonlinearFactorGraph& summarizedFactors, const Values& rootValues) = 0; + + /** + * Perform any required operations after the synchronization process finishes. + * Called by 'synchronize' + */ + virtual void postsync() {}; + +}; // ConcurrentSmoother + +}/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp new file mode 100644 index 000000000..57113830a --- /dev/null +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -0,0 +1,281 @@ +/* ---------------------------------------------------------------------------- + + * 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 LinearizedFactor.cpp + * @brief A dummy factor that allows a linear factor to act as a nonlinear factor + * @author Alex Cunningham + */ + +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +LinearizedGaussianFactor::LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Ordering& ordering, const Values& lin_points) { + // Extract the keys and linearization points + BOOST_FOREACH(const Index& idx, gaussian->keys()) { + // find full symbol + if (idx < ordering.size()) { + Key key = ordering.key(idx); + + // extract linearization point + assert(lin_points.exists(key)); + this->lin_points_.insert(key, lin_points.at(key)); + + // store keys + this->keys_.push_back(key); + } else { + throw std::runtime_error("LinearizedGaussianFactor: could not find index in decoder!"); + } + } +} + + + +/* ************************************************************************* */ +LinearizedJacobianFactor::LinearizedJacobianFactor() : Ab_(matrix_) { +} + +/* ************************************************************************* */ +LinearizedJacobianFactor::LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian, + const Ordering& ordering, const Values& lin_points) +: Base(jacobian, ordering, lin_points), Ab_(matrix_) { + + // Get the Ab matrix from the Jacobian factor, with any covariance baked in + AbMatrix fullMatrix = jacobian->matrix_augmented(true); + + // Create the dims array + size_t dims[jacobian->size() + 1]; + size_t index = 0; + for(JacobianFactor::const_iterator iter = jacobian->begin(); iter != jacobian->end(); ++iter) { + dims[index++] = jacobian->getDim(iter); + } + dims[index] = 1; + + // Update the BlockInfo accessor + BlockAb Ab(fullMatrix, dims, dims+jacobian->size()+1); + Ab.swap(Ab_); +} + +/* ************************************************************************* */ +void LinearizedJacobianFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { + + std::cout << s << std::endl; + + std::cout << "Nonlinear Keys: "; + BOOST_FOREACH(const Key& key, this->keys()) + std::cout << keyFormatter(key) << " "; + std::cout << std::endl; + + for(const_iterator key=begin(); key!=end(); ++key) + std::cout << boost::format("A[%1%]=\n")%keyFormatter(*key) << A(*key) << std::endl; + std::cout << "b=\n" << b() << std::endl; + + lin_points_.print("Linearization Point: "); +} + +/* ************************************************************************* */ +bool LinearizedJacobianFactor::equals(const NonlinearFactor& expected, double tol) const { + + const This *e = dynamic_cast (&expected); + if (e) { + + Matrix thisMatrix = this->Ab_.range(0, Ab_.nBlocks()); + Matrix rhsMatrix = e->Ab_.range(0, Ab_.nBlocks()); + + return Base::equals(expected, tol) + && lin_points_.equals(e->lin_points_, tol) + && equal_with_abs_tol(thisMatrix, rhsMatrix, tol); + } else { + return false; + } +} + +/* ************************************************************************* */ +double LinearizedJacobianFactor::error(const Values& c) const { + Vector errorVector = error_vector(c); + return 0.5 * errorVector.dot(errorVector); +} + +/* ************************************************************************* */ +boost::shared_ptr +LinearizedJacobianFactor::linearize(const Values& c, const Ordering& ordering) const { + + // Create the 'terms' data structure for the Jacobian constructor + std::vector > terms; + BOOST_FOREACH(Key key, keys()) { + terms.push_back(std::make_pair(ordering[key], this->A(key))); + } + + // compute rhs + Vector b = -error_vector(c); + + return boost::shared_ptr(new JacobianFactor(terms, b, noiseModel::Unit::Create(dim()))); +} + +/* ************************************************************************* */ +Vector LinearizedJacobianFactor::error_vector(const Values& c) const { + + Vector errorVector = -b(); + + BOOST_FOREACH(Key key, this->keys()) { + const Value& newPt = c.at(key); + const Value& linPt = lin_points_.at(key); + Vector d = linPt.localCoordinates_(newPt); + const constABlock A = this->A(key); + errorVector += A * d; + } + + return errorVector; +} + + + + + + +/* ************************************************************************* */ +LinearizedHessianFactor::LinearizedHessianFactor() : info_(matrix_) { +} + +/* ************************************************************************* */ +LinearizedHessianFactor::LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian, + const Ordering& ordering, const Values& lin_points) +: Base(hessian, ordering, lin_points), info_(matrix_) { + + // Copy the augmented matrix holding G, g, and f + Matrix fullMatrix = hessian->info(); + + // Create the dims array + size_t dims[hessian->size() + 1]; + size_t index = 0; + for(HessianFactor::const_iterator iter = hessian->begin(); iter != hessian->end(); ++iter) { + dims[index++] = hessian->getDim(iter); + } + dims[index] = 1; + + // Update the BlockInfo accessor + BlockInfo infoMatrix(fullMatrix, dims, dims+hessian->size()+1); + infoMatrix.swap(info_); +} + +/* ************************************************************************* */ +void LinearizedHessianFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { + + std::cout << s << std::endl; + + std::cout << "Nonlinear Keys: "; + BOOST_FOREACH(const Key& key, this->keys()) + std::cout << keyFormatter(key) << " "; + std::cout << std::endl; + + gtsam::print(Matrix(info_.range(0,info_.nBlocks(), 0,info_.nBlocks()).selfadjointView()), "Ab^T * Ab: "); + + lin_points_.print("Linearization Point: "); +} + +/* ************************************************************************* */ +bool LinearizedHessianFactor::equals(const NonlinearFactor& expected, double tol) const { + + const This *e = dynamic_cast (&expected); + if (e) { + + Matrix thisMatrix = this->info_.full().selfadjointView(); + thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; + Matrix rhsMatrix = e->info_.full().selfadjointView(); + rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; + + return Base::equals(expected, tol) + && lin_points_.equals(e->lin_points_, tol) + && equal_with_abs_tol(thisMatrix, rhsMatrix, tol); + } else { + return false; + } +} + +/* ************************************************************************* */ +double LinearizedHessianFactor::error(const Values& c) const { + + // Construct an error vector in key-order from the Values + Vector dx = zero(dim()); + size_t index = 0; + for(unsigned int i = 0; i < this->size(); ++i){ + Key key = this->keys()[i]; + const Value& newPt = c.at(key); + const Value& linPt = lin_points_.at(key); + dx.segment(index, linPt.dim()) = linPt.localCoordinates_(newPt); + index += linPt.dim(); + } + + // error 0.5*(f - 2*x'*g + x'*G*x) + double f = constantTerm(); + double xtg = dx.dot(linearTerm()); + double xGx = dx.transpose() * squaredTerm().selfadjointView() * dx; + + return 0.5 * (f - 2.0 * xtg + xGx); +} + +/* ************************************************************************* */ +boost::shared_ptr +LinearizedHessianFactor::linearize(const Values& c, const Ordering& ordering) const { + + // Use the ordering to convert the keys into indices; + std::vector js; + BOOST_FOREACH(Key key, this->keys()){ + js.push_back(ordering.at(key)); + } + + // Make a copy of the info matrix + Matrix newMatrix; + SymmetricBlockView newInfo(newMatrix); + newInfo.assignNoalias(info_); + + // Construct an error vector in key-order from the Values + Vector dx = zero(dim()); + size_t index = 0; + for(unsigned int i = 0; i < this->size(); ++i){ + Key key = this->keys()[i]; + const Value& newPt = c.at(key); + const Value& linPt = lin_points_.at(key); + dx.segment(index, linPt.dim()) = linPt.localCoordinates_(newPt); + index += linPt.dim(); + } + + // f2 = f1 - 2*dx'*g1 + dx'*G1*dx + //newInfo(this->size(), this->size())(0,0) += -2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView() * dx; + double f = constantTerm() - 2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView() * dx; + + // g2 = g1 - G1*dx + //newInfo.rangeColumn(0, this->size(), this->size(), 0) -= squaredTerm().selfadjointView() * dx; + Vector g = linearTerm() - squaredTerm().selfadjointView() * dx; + std::vector gs; + for(size_t i = 0; i < info_.nBlocks()-1; ++i) { + gs.push_back(g.segment(info_.offset(i), info_.offset(i+1) - info_.offset(i))); + } + + // G2 = G1 + // Do Nothing + std::vector Gs; + for(size_t i = 0; i < info_.nBlocks()-1; ++i) { + for(size_t j = i; j < info_.nBlocks()-1; ++j) { + Gs.push_back(info_(i,j)); + } + } + + // Create a Hessian Factor from the modified info matrix + //return boost::shared_ptr(new HessianFactor(js, newInfo)); + return boost::shared_ptr(new HessianFactor(js, Gs, gs, f)); +} + +} // \namespace aspn diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h new file mode 100644 index 000000000..f86d3bb2d --- /dev/null +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -0,0 +1,289 @@ +/* ---------------------------------------------------------------------------- + + * 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 LinearizedFactor.h + * @brief A dummy factor that allows a linear factor to act as a nonlinear factor + * @author Alex Cunningham + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * A base factor class for the Jacobian and Hessian linearized factors + */ +class LinearizedGaussianFactor : public NonlinearFactor { +public: + /** base type */ + typedef NonlinearFactor Base; + typedef LinearizedGaussianFactor This; + + /** shared pointer for convenience */ + typedef boost::shared_ptr shared_ptr; + +protected: + + /** linearization points for error calculation */ + Values lin_points_; + +public: + + /** default constructor for serialization */ + LinearizedGaussianFactor() {}; + + /** + * @param gaussian: A jacobian or hessian factor + * @param ordering: The full ordering used to linearize this factor + * @param lin_points: The linearization points for, at least, the variables used by this factor + */ + LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Ordering& ordering, const Values& lin_points); + + virtual ~LinearizedGaussianFactor() {}; + + // access functions + const Values& linearizationPoint() const { return lin_points_; } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("LinearizedGaussianFactor", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(lin_points_); + } + +}; + +/** + * A factor that takes a linear, Jacobian factor and inserts it into + * a nonlinear graph. + */ +class LinearizedJacobianFactor : public LinearizedGaussianFactor { + +public: + /** base type */ + typedef LinearizedGaussianFactor Base; + typedef LinearizedJacobianFactor This; + + /** shared pointer for convenience */ + typedef boost::shared_ptr shared_ptr; + + typedef Matrix AbMatrix; + typedef VerticalBlockView BlockAb; + typedef BlockAb::Block ABlock; + typedef BlockAb::constBlock constABlock; + typedef BlockAb::Column BVector; + typedef BlockAb::constColumn constBVector; + +protected: + +// // store components of a jacobian factor +// typedef std::map KeyMatrixMap; +// KeyMatrixMap matrices_; +// Vector b_; + + AbMatrix matrix_; // the full matrix corresponding to the factor + BlockAb Ab_; // the block view of the full matrix + +public: + + /** default constructor for serialization */ + LinearizedJacobianFactor(); + + /** + * @param jacobian: A jacobian factor + * @param ordering: The ordering used to linearize this factor + * @param lin_points: The linearization points for, at least, the variables used by this factor + */ + LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian, + const Ordering& ordering, const Values& lin_points); + + virtual ~LinearizedJacobianFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + // Testable + + /** print function */ + virtual void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** equals function with optional tolerance */ + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + + // access functions + const constBVector b() const { return Ab_.column(size(), 0); } + const constABlock A() const { return Ab_.range(0, size()); }; + const constABlock A(Key key) const { return Ab_(std::find(begin(), end(), key) - begin()); } + + /** get the dimension of the factor (number of rows on linearization) */ + size_t dim() const { return Ab_.rows(); }; + + /** Calculate the error of the factor */ + double error(const Values& c) const; + + /** + * linearize to a GaussianFactor + * Reimplemented from NoiseModelFactor to directly copy out the + * matrices and only update the RHS b with an updated residual + */ + boost::shared_ptr linearize( + const Values& c, const Ordering& ordering) const; + + /** (A*x-b) */ + Vector error_vector(const Values& c) const; + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("LinearizedJacobianFactor", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(Ab_); + } +}; + + + + +/** + * A factor that takes a linear, Hessian factor and inserts it into + * a nonlinear graph. + */ +class LinearizedHessianFactor : public LinearizedGaussianFactor { + +public: + /** base type */ + typedef LinearizedGaussianFactor Base; + typedef LinearizedHessianFactor This; + + /** shared pointer for convenience */ + typedef boost::shared_ptr shared_ptr; + + /** hessian block data types */ + typedef Matrix InfoMatrix; ///< The full augmented Hessian + typedef SymmetricBlockView BlockInfo; ///< A blockwise view of the Hessian + typedef BlockInfo::Block Block; ///< A block from the Hessian matrix + typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version) + typedef BlockInfo::Column Column; ///< A column containing the linear term h + typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version) + +protected: + + InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] + BlockInfo info_; ///< The block view of the full information matrix. + +public: + + /** default constructor for serialization */ + LinearizedHessianFactor(); + + /** + * Use this constructor with the ordering used to linearize the graph + * @param hessian: A hessian factor + * @param ordering: The ordering used to linearize this factor + * @param lin_points: The linearization points for, at least, the variables used by this factor + */ + LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian, + const Ordering& ordering, const Values& lin_points); + + virtual ~LinearizedHessianFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + + // Testable + + /** print function */ + virtual void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** equals function with optional tolerance */ + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; + + /** Return the constant term \f$ f \f$ as described above + * @return The constant term \f$ f \f$ + */ + double constantTerm() const { return info_(this->size(), this->size())(0,0); }; + + /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. + * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return The linear term \f$ g \f$ */ + constColumn linearTerm(const_iterator j) const { return info_.column(j - this->begin(), this->size(), 0); } + + /** Return the complete linear term \f$ g \f$ as described above. + * @return The linear term \f$ g \f$ */ + constColumn linearTerm() const { return info_.rangeColumn(0, this->size(), this->size(), 0); }; + + /** Return a view of the block at (j1,j2) of the upper-triangular part of the + * squared term \f$ H \f$, no data is copied. See HessianFactor class documentation + * above to explain that only the upper-triangular part of the information matrix is stored + * and returned by this function. + * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return A view of the requested block, not a copy. + */ + constBlock squaredTerm(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); } + + /** Return the upper-triangular part of the full squared term, as described above. + * See HessianFactor class documentation above to explain that only the + * upper-triangular part of the information matrix is stored and returned by this function. + */ + constBlock squaredTerm() const { return info_.range(0, this->size(), 0, this->size()); } + + + /** get the dimension of the factor (number of rows on linearization) */ + size_t dim() const { return matrix_.rows() - 1; }; + + /** Calculate the error of the factor */ + double error(const Values& c) const; + + /** + * linearize to a GaussianFactor + * Reimplemented from NoiseModelFactor to directly copy out the + * matrices and only update the RHS b with an updated residual + */ + boost::shared_ptr linearize( + const Values& c, const Ordering& ordering) const; + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("LinearizedHessianFactor", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(matrix_); + ar & BOOST_SERIALIZATION_NVP(info_); + } +}; + + + +} // \namespace aspn diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp new file mode 100644 index 000000000..c3480d2db --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -0,0 +1,181 @@ +/* ---------------------------------------------------------------------------- + + * 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 testIncrementalFixedLagSmoother.cpp + * @brief Unit tests for the Incremental Fixed-Lag Smoother + * @author Stephen Williams (swilliams8@gatech.edu) + * @date May 23, 2012 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const BatchFixedLagSmoother& smoother, const Key& key) { + + Ordering ordering = *fullgraph.orderingCOLAMD(fullinit); + GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); + GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); + VectorValues delta = optimize(gbn); + Values fullfinal = fullinit.retract(delta, ordering); + + Point2 expected = fullfinal.at(key); + Point2 actual = smoother.calculateEstimate(key); + + return assert_equal(expected, actual); +} + +/* ************************************************************************* */ +TEST_UNSAFE( BatchFixedLagSmoother, Example ) +{ + // Test the BatchFixedLagSmoother in a pure linear environment. Thus, full optimization and + // the BatchFixedLagSmoother should be identical (even with the linearized approximations at + // the end of the smoothing lag) + + SETDEBUG("BatchFixedLagSmoother update", false); + + // Set up parameters + SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + + // Create a Fixed-Lag Smoother + typedef BatchFixedLagSmoother::KeyTimestampMap Timestamps; + BatchFixedLagSmoother smoother(LevenbergMarquardtParams(), 7.0); + + // Create containers to keep the full graph + Values fullinit; + NonlinearFactorGraph fullgraph; + + + + // i keeps track of the time step + size_t i = 0; + + // Add a prior at time 0 and update the HMF + { + Key key0(0); + + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; + + newFactors.add(PriorFactor(key0, Point2(0.0, 0.0), odometerNoise)); + newValues.insert(key0, Point2(0.01, 0.01)); + newTimestamps[key0] = 0.0; + + fullgraph.push_back(newFactors); + fullinit.insert(newValues); + + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); + + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key0)); + + ++i; + } + + // Add odometry from time 0 to time 5 + while(i <= 5) { + Key key1(i-1); + Key key2(i); + + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; + + newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newValues.insert(key2, Point2(double(i)+0.1, -0.1)); + newTimestamps[key2] = double(i); + + fullgraph.push_back(newFactors); + fullinit.insert(newValues); + + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); + + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key2)); + + ++i; + } + + // Add odometry from time 5 to 6 to the HMF and a loop closure at time 5 to the TSM + { + // Add the odometry factor to the HMF + Key key1(i-1); + Key key2(i); + + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; + + newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.add(BetweenFactor(Key(2), Key(5), Point2(3.5, 0.0), loopNoise)); + newValues.insert(key2, Point2(double(i)+0.1, -0.1)); + newTimestamps[key2] = double(i); + + fullgraph.push_back(newFactors); + fullinit.insert(newValues); + + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); + + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key2)); + + ++i; + } + + // Add odometry from time 6 to time 15 + while(i <= 15) { + Key key1(i-1); + Key key2(i); + + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; + + newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newValues.insert(key2, Point2(double(i)+0.1, -0.1)); + newTimestamps[key2] = double(i); + + fullgraph.push_back(newFactors); + fullinit.insert(newValues); + + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); + + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key2)); + + ++i; + } + +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp new file mode 100644 index 000000000..2338502d3 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -0,0 +1,783 @@ +/* ---------------------------------------------------------------------------- + + * 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 testConcurrentBatchFilter.cpp + * @brief Unit tests for the Concurrent Batch Filter + * @author Stephen Williams (swilliams8@gatech.edu) + * @date Jan 5, 2013 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Set up initial pose, odometry difference, loop closure difference, and initialization errors +const Pose3 poseInitial; +const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); + +// Set up noise models for the factors +const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); + +// Create a derived class to allow testing protected member functions +class ConcurrentBatchFilterTester : public ConcurrentBatchFilter { +public: + ConcurrentBatchFilterTester(const LevenbergMarquardtParams& parameters, double lag) : ConcurrentBatchFilter(parameters, lag) { }; + virtual ~ConcurrentBatchFilterTester() { }; + + // Add accessors to the protected members + void presync() { ConcurrentBatchFilter::presync(); }; + void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors, Values& rootValues) { ConcurrentBatchFilter::getSummarizedFactors(summarizedFactors, rootValues); }; + void getSmootherFactors(NonlinearFactorGraph& smootherFactors, Values& smootherValues) { ConcurrentBatchFilter::getSmootherFactors(smootherFactors, smootherValues); }; + void synchronize(const NonlinearFactorGraph& summarizedFactors) { ConcurrentBatchFilter::synchronize(summarizedFactors); }; + void postsync() { ConcurrentBatchFilter::postsync(); }; +}; + +/* ************************************************************************* */ +bool hessian_equal(const NonlinearFactorGraph& expected, const NonlinearFactorGraph& actual, const Values& theta, double tol = 1e-9) { + + FastSet expectedKeys = expected.keys(); + FastSet actualKeys = actual.keys(); + + // Verify the set of keys in both graphs are the same + if(!std::equal(expectedKeys.begin(), expectedKeys.end(), actualKeys.begin())) + return false; + + // Create an ordering + Ordering ordering; + BOOST_FOREACH(Key key, expectedKeys) { + ordering.push_back(key); + } + + // Linearize each factor graph + GaussianFactorGraph expectedGaussian; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, expected) { + if(factor) + expectedGaussian.push_back( factor->linearize(theta, ordering) ); + } + GaussianFactorGraph actualGaussian; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, actual) { + if(factor) + actualGaussian.push_back( factor->linearize(theta, ordering) ); + } + + // Convert linear factor graph into a dense Hessian + Matrix expectedHessian = expectedGaussian.augmentedHessian(); + Matrix actualHessian = actualGaussian.augmentedHessian(); + + // Zero out the lower-right entry. This corresponds to a constant in the optimization, + // which does not affect the result. Further, in conversions between Jacobians and Hessians, + // this term is ignored. + expectedHessian(expectedHessian.rows()-1, expectedHessian.cols()-1) = 0.0; + actualHessian(actualHessian.rows()-1, actualHessian.cols()-1) = 0.0; + + // Compare Hessians + return assert_equal(expectedHessian, actualHessian, tol); +} + +///* ************************************************************************* */ +void CreateFactors(NonlinearFactorGraph& graph, Values& theta, ConcurrentBatchFilter::KeyTimestampMap& timestamps, size_t index1 = 0, size_t index2 = 1) { + + // Calculate all poses + Pose3 poses[20]; + poses[0] = poseInitial; + for(size_t index = 1; index < 20; ++index) { + poses[index] = poses[index-1].compose(poseOdometry); + } + + // Create all keys + Key keys[20]; + for(size_t index = 0; index < 20; ++index) { + keys[index] = Symbol('X', index); + } + + // Create factors that will form a specific tree structure + // Loop over the included timestamps + for(size_t index = index1; index < index2; ++index) { + + switch(index) { + case 0: + { + graph.add(PriorFactor(keys[0], poses[0], noisePrior)); + // Add new variables + theta.insert(keys[0], poses[0].compose(poseError)); + timestamps[keys[0]] = double(0); + break; + } + case 1: + { + // Add odometry factor between 0 and 1 + Pose3 poseDelta = poses[0].between(poses[1]); + graph.add(BetweenFactor(keys[0], keys[1], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[1], poses[1].compose(poseError)); + timestamps[keys[1]] = double(1); + break; + } + case 2: + { + break; + } + case 3: + { + // Add odometry factor between 1 and 3 + Pose3 poseDelta = poses[1].between(poses[3]); + graph.add(BetweenFactor(keys[1], keys[3], poseDelta, noiseOdometery)); + // Add odometry factor between 2 and 3 + poseDelta = poses[2].between(poses[3]); + graph.add(BetweenFactor(keys[2], keys[3], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[2], poses[2].compose(poseError)); + timestamps[keys[2]] = double(2); + theta.insert(keys[3], poses[3].compose(poseError)); + timestamps[keys[3]] = double(3); + break; + } + case 4: + { + break; + } + case 5: + { + // Add odometry factor between 3 and 5 + Pose3 poseDelta = poses[3].between(poses[5]); + graph.add(BetweenFactor(keys[3], keys[5], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[5], poses[5].compose(poseError)); + timestamps[keys[5]] = double(5); + break; + } + case 6: + { + // Add odometry factor between 3 and 6 + Pose3 poseDelta = poses[3].between(poses[6]); + graph.add(BetweenFactor(keys[3], keys[6], poseDelta, noiseOdometery)); + // Add odometry factor between 5 and 6 + poseDelta = poses[5].between(poses[6]); + graph.add(BetweenFactor(keys[5], keys[6], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[6], poses[6].compose(poseError)); + timestamps[keys[6]] = double(6); + break; + } + case 7: + { + // Add odometry factor between 4 and 7 + Pose3 poseDelta = poses[4].between(poses[7]); + graph.add(BetweenFactor(keys[4], keys[7], poseDelta, noiseOdometery)); + // Add odometry factor between 6 and 7 + poseDelta = poses[6].between(poses[7]); + graph.add(BetweenFactor(keys[6], keys[7], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[4], poses[4].compose(poseError)); + timestamps[keys[4]] = double(4); + theta.insert(keys[7], poses[7].compose(poseError)); + timestamps[keys[7]] = double(7); + break; + } + case 8: + break; + + case 9: + { + // Add odometry factor between 6 and 9 + Pose3 poseDelta = poses[6].between(poses[9]); + graph.add(BetweenFactor(keys[6], keys[9], poseDelta, noiseOdometery)); + // Add odometry factor between 7 and 9 + poseDelta = poses[7].between(poses[9]); + graph.add(BetweenFactor(keys[7], keys[9], poseDelta, noiseOdometery)); + // Add odometry factor between 8 and 9 + poseDelta = poses[8].between(poses[9]); + graph.add(BetweenFactor(keys[8], keys[9], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[8], poses[8].compose(poseError)); + timestamps[keys[8]] = double(8); + theta.insert(keys[9], poses[9].compose(poseError)); + timestamps[keys[9]] = double(9); + break; + } + case 10: + { + // Add odometry factor between 9 and 10 + Pose3 poseDelta = poses[9].between(poses[10]); + graph.add(BetweenFactor(keys[9], keys[10], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[10], poses[10].compose(poseError)); + timestamps[keys[10]] = double(10); + break; + } + case 11: + { + // Add odometry factor between 10 and 11 + Pose3 poseDelta = poses[10].between(poses[11]); + graph.add(BetweenFactor(keys[10], keys[11], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[11], poses[11].compose(poseError)); + timestamps[keys[11]] = double(11); + break; + } + case 12: + { + // Add odometry factor between 7 and 12 + Pose3 poseDelta = poses[7].between(poses[12]); + graph.add(BetweenFactor(keys[7], keys[12], poseDelta, noiseOdometery)); + // Add odometry factor between 9 and 12 + poseDelta = poses[9].between(poses[12]); + graph.add(BetweenFactor(keys[9], keys[12], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[12], poses[12].compose(poseError)); + timestamps[keys[12]] = double(12); + break; + } + + + + + + case 13: + { + // Add odometry factor between 10 and 13 + Pose3 poseDelta = poses[10].between(poses[13]); + graph.add(BetweenFactor(keys[10], keys[13], poseDelta, noiseOdometery)); + // Add odometry factor between 12 and 13 + poseDelta = poses[12].between(poses[13]); + graph.add(BetweenFactor(keys[12], keys[13], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[13], poses[13].compose(poseError)); + timestamps[keys[13]] = double(13); + break; + } + case 14: + { + // Add odometry factor between 11 and 14 + Pose3 poseDelta = poses[11].between(poses[14]); + graph.add(BetweenFactor(keys[11], keys[14], poseDelta, noiseOdometery)); + // Add odometry factor between 13 and 14 + poseDelta = poses[13].between(poses[14]); + graph.add(BetweenFactor(keys[13], keys[14], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[14], poses[14].compose(poseError)); + timestamps[keys[14]] = double(14); + break; + } + case 15: + break; + + case 16: + { + // Add odometry factor between 13 and 16 + Pose3 poseDelta = poses[13].between(poses[16]); + graph.add(BetweenFactor(keys[13], keys[16], poseDelta, noiseOdometery)); + // Add odometry factor between 14 and 16 + poseDelta = poses[14].between(poses[16]); + graph.add(BetweenFactor(keys[14], keys[16], poseDelta, noiseOdometery)); + // Add odometry factor between 15 and 16 + poseDelta = poses[15].between(poses[16]); + graph.add(BetweenFactor(keys[15], keys[16], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[15], poses[15].compose(poseError)); + timestamps[keys[15]] = double(15); + theta.insert(keys[16], poses[16].compose(poseError)); + timestamps[keys[16]] = double(16); + break; + } + case 17: + { + // Add odometry factor between 16 and 17 + Pose3 poseDelta = poses[16].between(poses[17]); + graph.add(BetweenFactor(keys[16], keys[17], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[17], poses[17].compose(poseError)); + timestamps[keys[17]] = double(17); + break; + } + case 18: + { + // Add odometry factor between 17 and 18 + Pose3 poseDelta = poses[17].between(poses[18]); + graph.add(BetweenFactor(keys[17], keys[18], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[18], poses[18].compose(poseError)); + timestamps[keys[18]] = double(18); + break; + } + case 19: + { + // Add odometry factor between 14 and 19 + Pose3 poseDelta = poses[14].between(poses[19]); + graph.add(BetweenFactor(keys[14], keys[19], poseDelta, noiseOdometery)); + // Add odometry factor between 16 and 19 + poseDelta = poses[16].between(poses[19]); + graph.add(BetweenFactor(keys[16], keys[19], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[19], poses[19].compose(poseError)); + timestamps[keys[19]] = double(19); + break; + } + + } + } + + return; +} + +/* ************************************************************************* */ +Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, const Values& rootValues = Values()) { + + // Create an L-M optimizer + LevenbergMarquardtParams parameters; + parameters.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_QR; + + LevenbergMarquardtOptimizer optimizer(graph, theta, parameters); + + // Use a custom optimization loop so the linearization points can be controlled + double currentError; + do { + // Force variables associated with root keys to keep the same linearization point + if(rootValues.size() > 0) { + // Put the old values of the root keys back into the optimizer state + optimizer.state().values.update(rootValues); + // Update the error value with the new theta + optimizer.state().error = graph.error(optimizer.state().values); + } + + // Do next iteration + currentError = optimizer.error(); + optimizer.iterate(); + + } while(optimizer.iterations() < parameters.maxIterations && + !checkConvergence(parameters.relativeErrorTol, parameters.absoluteErrorTol, + parameters.errorTol, currentError, optimizer.error(), parameters.verbosity)); + + // return the final optimized values + return optimizer.values(); +} + +/* ************************************************************************* */ +void FindFactorsWithAny(const std::set& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) { + + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) { + NonlinearFactor::const_iterator key = factor->begin(); + while((key != factor->end()) && (!std::binary_search(keys.begin(), keys.end(), *key))) { + ++key; + } + if(key != factor->end()) { + destinationFactors.push_back(factor); + } + } + +} + +/* ************************************************************************* */ +void FindFactorsWithOnly(const std::set& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) { + + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) { + NonlinearFactor::const_iterator key = factor->begin(); + while((key != factor->end()) && (std::binary_search(keys.begin(), keys.end(), *key))) { + ++key; + } + if(key == factor->end()) { + destinationFactors.push_back(factor); + } + } + +} + +/* ************************************************************************* */ +typedef BayesTree::sharedClique Clique; +void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = "") { + std::cout << indent << "P( "; + BOOST_FOREACH(Index index, clique->conditional()->frontals()){ + std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; + } + if(clique->conditional()->nrParents() > 0) { + std::cout << "| "; + } + BOOST_FOREACH(Index index, clique->conditional()->parents()){ + std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; + } + std::cout << ")" << std::endl; + + BOOST_FOREACH(const Clique& child, clique->children()) { + SymbolicPrintTree(child, ordering, indent+" "); + } +} + +/* ************************************************************************* */ +TEST_UNSAFE( ConcurrentBatchFilter, update_Batch ) +{ + // Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment. + // Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical + // This tests adds all of the factors to the filter at once (i.e. batch) + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + double lag = 4.0; + + // Create a Concurrent Batch Filter + ConcurrentBatchFilter filter(parameters, lag); + + // Create containers to keep the full graph + Values fullTheta; + NonlinearFactorGraph fullGraph; + ConcurrentBatchFilter::KeyTimestampMap fullTimestamps; + + // Create all factors + CreateFactors(fullGraph, fullTheta, fullTimestamps, 0, 20); + + // Optimize with Concurrent Batch Filter + filter.update(fullGraph, fullTheta, fullTimestamps); + Values actual = filter.calculateEstimate(); + + // Optimize with L-M + Values expected = BatchOptimize(fullGraph, fullTheta); + + // Check smoother versus batch + CHECK(assert_equal(expected, actual, 1e-4)); +} + +/* ************************************************************************* */ +TEST_UNSAFE( ConcurrentBatchFilter, update_Incremental ) +{ + // Test the 'update' function of the ConcurrentBatchFilter in a nonlinear environment. + // Thus, a full L-M optimization and the ConcurrentBatchFilter results should be identical + // This tests adds the factors to the filter as they are created (i.e. incrementally) + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + double lag = 4.0; + + // Create a Concurrent Batch Filter + ConcurrentBatchFilter filter(parameters, lag); + + // Create containers to keep the full graph + Values fullTheta; + NonlinearFactorGraph fullGraph; + + // Add odometry from time 0 to time 10 + for(size_t i = 0; i < 20; ++i) { + // Create containers to keep the new factors + Values newTheta; + NonlinearFactorGraph newGraph; + ConcurrentBatchFilter::KeyTimestampMap newTimestamps; + + // Create factors + CreateFactors(newGraph, newTheta, newTimestamps, i, i+1); + + // Add these entries to the filter + filter.update(newGraph, newTheta, newTimestamps); + Values actual = filter.calculateEstimate(); + + // Add these entries to the full batch version + fullGraph.push_back(newGraph); + fullTheta.insert(newTheta); + Values expected = BatchOptimize(fullGraph, fullTheta); + fullTheta = expected; + + // Compare filter solution with full batch + CHECK(assert_equal(expected, actual, 1e-4)); + } + +} + +/* ************************************************************************* */ +TEST_UNSAFE( ConcurrentBatchFilter, synchronize ) +{ + // Test the 'synchronize' function of the ConcurrentBatchFilter in a nonlinear environment. + // The filter is operating on a known tree structure, so the factors and summarization can + // be predicted for testing purposes + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + double lag = 4.0; + + // Create a Concurrent Batch Filter + ConcurrentBatchFilterTester filter(parameters, lag); + + // Create containers to keep the full graph + Values newTheta, fullTheta; + NonlinearFactorGraph newGraph, fullGraph; + ConcurrentBatchFilter::KeyTimestampMap newTimestamps; + + // Create all factors + CreateFactors(newGraph, newTheta, newTimestamps, 0, 13); + fullTheta.insert(newTheta); + fullGraph.push_back(newGraph); + + // Optimize with Concurrent Batch Filter + filter.update(newGraph, newTheta, newTimestamps); + Values updatedTheta = filter.calculateEstimate(); + + // Eliminate the factor graph into a Bayes Tree to create the summarized factors + // Create Ordering + std::map constraints; + constraints[Symbol('X', 8)] = 1; + constraints[Symbol('X', 9)] = 1; + constraints[Symbol('X', 10)] = 1; + constraints[Symbol('X', 11)] = 1; + constraints[Symbol('X', 12)] = 1; + Ordering ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints); + // Linearize into a Gaussian Factor Graph + GaussianFactorGraph linearGraph = *fullGraph.linearize(fullTheta, ordering); + // Eliminate into a Bayes Net with iSAM2-type cliques + JunctionTree jt(linearGraph); + ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR); + BayesTree bayesTree; + bayesTree.insert(root); + + // std::cout << "BAYES TREE:" << std::endl; + // SymbolicPrintTree(root, ordering.invert(), " "); + + // BAYES TREE: + // P( X7 X9 X12 ) + // P( X10 | X9 ) + // P( X11 | X10 ) + // P( X8 | X9 ) + // P( X6 | X7 X9 ) + // P( X3 X5 | X6 ) + // P( X2 | X3 ) + // P( X1 | X3 ) + // P( X0 | X1 ) + // P( X4 | X7 ) + + // Extract the nonlinear factors that should be sent to the smoother + std::vector smootherKeys; + smootherKeys.push_back(Symbol('X', 0)); + smootherKeys.push_back(Symbol('X', 1)); + smootherKeys.push_back(Symbol('X', 2)); + smootherKeys.push_back(Symbol('X', 3)); + smootherKeys.push_back(Symbol('X', 4)); + smootherKeys.push_back(Symbol('X', 5)); + smootherKeys.push_back(Symbol('X', 6)); + NonlinearFactorGraph expectedSmootherFactors; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { + if(std::find_first_of(factor->begin(), factor->end(), smootherKeys.begin(), smootherKeys.end()) != factor->end()) { + expectedSmootherFactors.push_back(factor); + } + } + + // Extract smoother values + Values expectedSmootherValues; + expectedSmootherValues.insert(Symbol('X', 0), updatedTheta.at(Symbol('X', 0))); + expectedSmootherValues.insert(Symbol('X', 1), updatedTheta.at(Symbol('X', 1))); + expectedSmootherValues.insert(Symbol('X', 2), updatedTheta.at(Symbol('X', 2))); + expectedSmootherValues.insert(Symbol('X', 3), updatedTheta.at(Symbol('X', 3))); + expectedSmootherValues.insert(Symbol('X', 4), updatedTheta.at(Symbol('X', 4))); + expectedSmootherValues.insert(Symbol('X', 5), updatedTheta.at(Symbol('X', 5))); + expectedSmootherValues.insert(Symbol('X', 6), updatedTheta.at(Symbol('X', 6))); + + // Extract the filter summarized factors + // Cached factors that represent the filter side (i.e. the X8 and X10 clique) + NonlinearFactorGraph expectedFilterSumarization; + expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 8)))->cachedFactor()), ordering, fullTheta)); + expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 10)))->cachedFactor()), ordering, fullTheta)); + // And any factors that involve only the root (X7, X9, X12) + std::vector rootKeys; + rootKeys.push_back(Symbol('X', 7)); + rootKeys.push_back(Symbol('X', 9)); + rootKeys.push_back(Symbol('X', 12)); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { + size_t count = 0; + BOOST_FOREACH(Key key, *factor) { + if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count; + } + if(count == factor->size()) expectedFilterSumarization.push_back(factor); + } + + // Extract the new root values + Values expectedRootValues; + expectedRootValues.insert(Symbol('X', 7), updatedTheta.at(Symbol('X', 7))); + expectedRootValues.insert(Symbol('X', 9), updatedTheta.at(Symbol('X', 9))); + expectedRootValues.insert(Symbol('X', 12), updatedTheta.at(Symbol('X', 12))); + + + + // Start the synchronization process + NonlinearFactorGraph actualSmootherFactors, actualFilterSumarization, smootherSummarization; + Values actualSmootherValues, actualRootValues; + filter.presync(); + filter.synchronize(smootherSummarization); // Supplying an empty factor graph here + filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues); + filter.getSummarizedFactors(actualFilterSumarization, actualRootValues); + filter.postsync(); + + + + // Compare filter sync variables versus the expected + CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, updatedTheta, 1e-8)); + CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-4)); + CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, updatedTheta, 1e-9)); + CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4)); + + + + + + + // Now add additional factors to the filter and re-sync + newGraph.resize(0); newTheta.clear(); newTimestamps.clear(); + CreateFactors(newGraph, newTheta, newTimestamps, 13, 20); + fullTheta.insert(newTheta); + fullGraph.push_back(newGraph); + + // Optimize with Concurrent Batch Filter + filter.update(newGraph, newTheta, newTimestamps); + updatedTheta = filter.calculateEstimate(); + + // Eliminate the factor graph into a Bayes Tree to create the summarized factors + // Create Ordering + constraints.clear(); + constraints[Symbol('X', 15)] = 1; + constraints[Symbol('X', 16)] = 1; + constraints[Symbol('X', 17)] = 1; + constraints[Symbol('X', 18)] = 1; + constraints[Symbol('X', 19)] = 1; + ordering = *fullGraph.orderingCOLAMDConstrained(fullTheta, constraints); + // Linearize into a Gaussian Factor Graph + linearGraph = *fullGraph.linearize(fullTheta, ordering); + // Eliminate into a Bayes Net with iSAM2-type cliques + jt = JunctionTree(linearGraph); + root = jt.eliminate(EliminateQR); + bayesTree = BayesTree(); + bayesTree.insert(root); + + // std::cout << "BAYES TREE:" << std::endl; + // SymbolicPrintTree(root, ordering.invert(), " "); + + // BAYES TREE: + // P( X14 X16 X19 ) + // P( X17 | X16 ) + // P( X18 | X17 ) + // P( X15 | X16 ) + // P( X13 | X14 X16 ) + // P( X11 | X13 X14 ) + // P( X10 | X11 X13 ) + // P( X12 | X10 X13 ) + // P( X9 | X12 X10 ) + // P( X7 | X9 X12 ) + // P( X6 | X7 X9 ) + // P( X3 X5 | X6 ) + // P( X2 | X3 ) + // P( X1 | X3 ) + // P( X0 | X1 ) + // P( X4 | X7 ) + // P( X8 | X9 ) + + // Extract the cached factors for X4 and X6. These are the new smoother summarized factors + // TODO: I'm concerned about the linearization point used to create these factors. It may need to be the updated lin points? + smootherSummarization.resize(0); + smootherSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 4)))->cachedFactor()), ordering, fullTheta)); + smootherSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor()), ordering, fullTheta)); + + + + + + // Extract the nonlinear factors that should be sent to the smoother + smootherKeys.clear(); + smootherKeys.push_back(Symbol('X', 8)); + smootherKeys.push_back(Symbol('X', 10)); + smootherKeys.push_back(Symbol('X', 11)); + smootherKeys.push_back(Symbol('X', 13)); + expectedSmootherFactors.resize(0); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { + if(std::find_first_of(factor->begin(), factor->end(), smootherKeys.begin(), smootherKeys.end()) != factor->end()) { + expectedSmootherFactors.push_back(factor); + } + } + // And any factors that involve only the old root (X7, X9, X12) + rootKeys.clear(); + rootKeys.push_back(Symbol('X', 7)); + rootKeys.push_back(Symbol('X', 9)); + rootKeys.push_back(Symbol('X', 12)); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { + size_t count = 0; + BOOST_FOREACH(Key key, *factor) { + if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count; + } + if(count == factor->size()) expectedSmootherFactors.push_back(factor); + } + + + // Extract smoother Values + expectedSmootherValues.clear(); + expectedSmootherValues.insert(Symbol('X', 7), updatedTheta.at(Symbol('X', 7))); + expectedSmootherValues.insert(Symbol('X', 8), updatedTheta.at(Symbol('X', 8))); + expectedSmootherValues.insert(Symbol('X', 9), updatedTheta.at(Symbol('X', 9))); + expectedSmootherValues.insert(Symbol('X', 10), updatedTheta.at(Symbol('X', 10))); + expectedSmootherValues.insert(Symbol('X', 11), updatedTheta.at(Symbol('X', 11))); + expectedSmootherValues.insert(Symbol('X', 12), updatedTheta.at(Symbol('X', 12))); + expectedSmootherValues.insert(Symbol('X', 13), updatedTheta.at(Symbol('X', 13))); + + // Extract the filter summarized factors + // Cached factors that represent the filter side (i.e. the X15 and X17 clique) + expectedFilterSumarization.resize(0); + expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 15)))->cachedFactor()), ordering, fullTheta)); + expectedFilterSumarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 17)))->cachedFactor()), ordering, fullTheta)); + // And any factors that involve only the root (X14, X16, X17) + rootKeys.clear(); + rootKeys.push_back(Symbol('X', 14)); + rootKeys.push_back(Symbol('X', 16)); + rootKeys.push_back(Symbol('X', 19)); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, fullGraph) { + size_t count = 0; + BOOST_FOREACH(Key key, *factor) { + if(std::binary_search(rootKeys.begin(), rootKeys.end(), key)) ++count; + } + if(count == factor->size()) expectedFilterSumarization.push_back(factor); + } + + // Extract the new root keys + expectedRootValues.clear(); + expectedRootValues.insert(Symbol('X', 14), updatedTheta.at(Symbol('X', 14))); + expectedRootValues.insert(Symbol('X', 16), updatedTheta.at(Symbol('X', 16))); + expectedRootValues.insert(Symbol('X', 19), updatedTheta.at(Symbol('X', 19))); + + + + // Start the synchronization process + actualSmootherFactors.resize(0); actualFilterSumarization.resize(0); + actualSmootherValues.clear(); actualRootValues.clear(); + filter.presync(); + filter.synchronize(smootherSummarization); + filter.getSmootherFactors(actualSmootherFactors, actualSmootherValues); + filter.getSummarizedFactors(actualFilterSumarization, actualRootValues); + filter.postsync(); + + + + // Compare filter sync variables versus the expected + CHECK(hessian_equal(expectedSmootherFactors, actualSmootherFactors, updatedTheta, 1e-8)); + CHECK(assert_equal(expectedSmootherValues, actualSmootherValues, 1e-4)); + CHECK(hessian_equal(expectedFilterSumarization, actualFilterSumarization, updatedTheta, 1e-8)); + CHECK(assert_equal(expectedRootValues, actualRootValues, 1e-4)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp new file mode 100644 index 000000000..1a63c40a7 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -0,0 +1,708 @@ +/* ---------------------------------------------------------------------------- + + * 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 testConcurrentBatchSmoother.cpp + * @brief Unit tests for the Concurrent Batch Smoother + * @author Stephen Williams (swilliams8@gatech.edu) + * @date Jan 5, 2013 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Set up initial pose, odometry difference, loop closure difference, and initialization errors +const Pose3 poseInitial; +const Pose3 poseOdometry( Rot3::RzRyRx(Vector_(3, 0.05, 0.10, -0.75)), Point3(1.0, -0.25, 0.10) ); +const Pose3 poseError( Rot3::RzRyRx(Vector_(3, 0.01, 0.02, -0.1)), Point3(0.05, -0.05, 0.02) ); + +// Set up noise models for the factors +const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +const SharedDiagonal noiseOdometery = noiseModel::Diagonal::Sigmas(Vector_(6, 0.1, 0.1, 0.1, 0.5, 0.5, 0.5)); +const SharedDiagonal noiseLoop = noiseModel::Diagonal::Sigmas(Vector_(6, 0.25, 0.25, 0.25, 1.0, 1.0, 1.0)); + +// Create a derived class to allow testing protected member functions +class ConcurrentBatchSmootherTester : public ConcurrentBatchSmoother { +public: + ConcurrentBatchSmootherTester(const LevenbergMarquardtParams& parameters) : ConcurrentBatchSmoother(parameters) { }; + virtual ~ConcurrentBatchSmootherTester() { }; + + // Add accessors to the protected members + void presync() { + ConcurrentBatchSmoother::presync(); + }; + void getSummarizedFactors(NonlinearFactorGraph& summarizedFactors) { + ConcurrentBatchSmoother::getSummarizedFactors(summarizedFactors); + }; + void synchronize(const NonlinearFactorGraph& smootherFactors, const Values& smootherValues, const NonlinearFactorGraph& summarizedFactors, const Values& rootValues) { + ConcurrentBatchSmoother::synchronize(smootherFactors, smootherValues, summarizedFactors, rootValues); + }; + void postsync() { + ConcurrentBatchSmoother::postsync(); + }; +}; + +/* ************************************************************************* */ +bool hessian_equal(const NonlinearFactorGraph& expected, const NonlinearFactorGraph& actual, const Values& theta, double tol = 1e-9) { + + FastSet expectedKeys = expected.keys(); + FastSet actualKeys = actual.keys(); + + // Verify the set of keys in both graphs are the same + if(!std::equal(expectedKeys.begin(), expectedKeys.end(), actualKeys.begin())) + return false; + + // Create an ordering + Ordering ordering; + BOOST_FOREACH(Key key, expectedKeys) { + ordering.push_back(key); + } + + // Linearize each factor graph + GaussianFactorGraph expectedGaussian; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, expected) { + if(factor) + expectedGaussian.push_back( factor->linearize(theta, ordering) ); + } + GaussianFactorGraph actualGaussian; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, actual) { + if(factor) + actualGaussian.push_back( factor->linearize(theta, ordering) ); + } + + // Convert linear factor graph into a dense Hessian + Matrix expectedHessian = expectedGaussian.augmentedHessian(); + Matrix actualHessian = actualGaussian.augmentedHessian(); + + // Zero out the lower-right entry. This corresponds to a constant in the optimization, + // which does not affect the result. Further, in conversions between Jacobians and Hessians, + // this term is ignored. + expectedHessian(expectedHessian.rows()-1, expectedHessian.cols()-1) = 0.0; + actualHessian(actualHessian.rows()-1, actualHessian.cols()-1) = 0.0; + + // Compare Hessians + return assert_equal(expectedHessian, actualHessian, tol); +} + +///* ************************************************************************* */ +void CreateFactors(NonlinearFactorGraph& graph, Values& theta, size_t index1 = 0, size_t index2 = 1) { + + // Calculate all poses + Pose3 poses[20]; + poses[0] = poseInitial; + for(size_t index = 1; index < 20; ++index) { + poses[index] = poses[index-1].compose(poseOdometry); + } + + // Create all keys + Key keys[20]; + for(size_t index = 0; index < 20; ++index) { + keys[index] = Symbol('X', index); + } + + // Create factors that will form a specific tree structure + // Loop over the included timestamps + for(size_t index = index1; index < index2; ++index) { + + switch(index) { + case 0: + { + graph.add(PriorFactor(keys[0], poses[0], noisePrior)); + // Add new variables + theta.insert(keys[0], poses[0].compose(poseError)); + break; + } + case 1: + { + // Add odometry factor between 0 and 1 + Pose3 poseDelta = poses[0].between(poses[1]); + graph.add(BetweenFactor(keys[0], keys[1], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[1], poses[1].compose(poseError)); + break; + } + case 2: + { + break; + } + case 3: + { + // Add odometry factor between 1 and 3 + Pose3 poseDelta = poses[1].between(poses[3]); + graph.add(BetweenFactor(keys[1], keys[3], poseDelta, noiseOdometery)); + // Add odometry factor between 2 and 3 + poseDelta = poses[2].between(poses[3]); + graph.add(BetweenFactor(keys[2], keys[3], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[2], poses[2].compose(poseError)); + theta.insert(keys[3], poses[3].compose(poseError)); + break; + } + case 4: + { + break; + } + case 5: + { + // Add odometry factor between 3 and 5 + Pose3 poseDelta = poses[3].between(poses[5]); + graph.add(BetweenFactor(keys[3], keys[5], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[5], poses[5].compose(poseError)); + break; + } + case 6: + { + // Add odometry factor between 3 and 6 + Pose3 poseDelta = poses[3].between(poses[6]); + graph.add(BetweenFactor(keys[3], keys[6], poseDelta, noiseOdometery)); + // Add odometry factor between 5 and 6 + poseDelta = poses[5].between(poses[6]); + graph.add(BetweenFactor(keys[5], keys[6], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[6], poses[6].compose(poseError)); + break; + } + case 7: + { + // Add odometry factor between 4 and 7 + Pose3 poseDelta = poses[4].between(poses[7]); + graph.add(BetweenFactor(keys[4], keys[7], poseDelta, noiseOdometery)); + // Add odometry factor between 6 and 7 + poseDelta = poses[6].between(poses[7]); + graph.add(BetweenFactor(keys[6], keys[7], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[4], poses[4].compose(poseError)); + theta.insert(keys[7], poses[7].compose(poseError)); + break; + } + case 8: + break; + + case 9: + { + // Add odometry factor between 6 and 9 + Pose3 poseDelta = poses[6].between(poses[9]); + graph.add(BetweenFactor(keys[6], keys[9], poseDelta, noiseOdometery)); + // Add odometry factor between 7 and 9 + poseDelta = poses[7].between(poses[9]); + graph.add(BetweenFactor(keys[7], keys[9], poseDelta, noiseOdometery)); + // Add odometry factor between 8 and 9 + poseDelta = poses[8].between(poses[9]); + graph.add(BetweenFactor(keys[8], keys[9], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[8], poses[8].compose(poseError)); + theta.insert(keys[9], poses[9].compose(poseError)); + break; + } + case 10: + { + // Add odometry factor between 9 and 10 + Pose3 poseDelta = poses[9].between(poses[10]); + graph.add(BetweenFactor(keys[9], keys[10], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[10], poses[10].compose(poseError)); + break; + } + case 11: + { + // Add odometry factor between 10 and 11 + Pose3 poseDelta = poses[10].between(poses[11]); + graph.add(BetweenFactor(keys[10], keys[11], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[11], poses[11].compose(poseError)); + break; + } + case 12: + { + // Add odometry factor between 7 and 12 + Pose3 poseDelta = poses[7].between(poses[12]); + graph.add(BetweenFactor(keys[7], keys[12], poseDelta, noiseOdometery)); + // Add odometry factor between 9 and 12 + poseDelta = poses[9].between(poses[12]); + graph.add(BetweenFactor(keys[9], keys[12], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[12], poses[12].compose(poseError)); + break; + } + + + + + + case 13: + { + // Add odometry factor between 10 and 13 + Pose3 poseDelta = poses[10].between(poses[13]); + graph.add(BetweenFactor(keys[10], keys[13], poseDelta, noiseOdometery)); + // Add odometry factor between 12 and 13 + poseDelta = poses[12].between(poses[13]); + graph.add(BetweenFactor(keys[12], keys[13], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[13], poses[13].compose(poseError)); + break; + } + case 14: + { + // Add odometry factor between 11 and 14 + Pose3 poseDelta = poses[11].between(poses[14]); + graph.add(BetweenFactor(keys[11], keys[14], poseDelta, noiseOdometery)); + // Add odometry factor between 13 and 14 + poseDelta = poses[13].between(poses[14]); + graph.add(BetweenFactor(keys[13], keys[14], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[14], poses[14].compose(poseError)); + break; + } + case 15: + break; + + case 16: + { + // Add odometry factor between 13 and 16 + Pose3 poseDelta = poses[13].between(poses[16]); + graph.add(BetweenFactor(keys[13], keys[16], poseDelta, noiseOdometery)); + // Add odometry factor between 14 and 16 + poseDelta = poses[14].between(poses[16]); + graph.add(BetweenFactor(keys[14], keys[16], poseDelta, noiseOdometery)); + // Add odometry factor between 15 and 16 + poseDelta = poses[15].between(poses[16]); + graph.add(BetweenFactor(keys[15], keys[16], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[15], poses[15].compose(poseError)); + theta.insert(keys[16], poses[16].compose(poseError)); + break; + } + case 17: + { + // Add odometry factor between 16 and 17 + Pose3 poseDelta = poses[16].between(poses[17]); + graph.add(BetweenFactor(keys[16], keys[17], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[17], poses[17].compose(poseError)); + break; + } + case 18: + { + // Add odometry factor between 17 and 18 + Pose3 poseDelta = poses[17].between(poses[18]); + graph.add(BetweenFactor(keys[17], keys[18], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[18], poses[18].compose(poseError)); + break; + } + case 19: + { + // Add odometry factor between 14 and 19 + Pose3 poseDelta = poses[14].between(poses[19]); + graph.add(BetweenFactor(keys[14], keys[19], poseDelta, noiseOdometery)); + // Add odometry factor between 16 and 19 + poseDelta = poses[16].between(poses[19]); + graph.add(BetweenFactor(keys[16], keys[19], poseDelta, noiseOdometery)); + // Add new variables + theta.insert(keys[19], poses[19].compose(poseError)); + break; + } + + } + } + + return; +} + +/* ************************************************************************* */ +Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, const Values& rootValues = Values()) { + + // Create an L-M optimizer + LevenbergMarquardtParams parameters; + parameters.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_QR; + + LevenbergMarquardtOptimizer optimizer(graph, theta, parameters); + + // Use a custom optimization loop so the linearization points can be controlled + double currentError; + do { + // Force variables associated with root keys to keep the same linearization point + if(rootValues.size() > 0) { + // Put the old values of the root keys back into the optimizer state + optimizer.state().values.update(rootValues); + // Update the error value with the new theta + optimizer.state().error = graph.error(optimizer.state().values); + } + + // Do next iteration + currentError = optimizer.error(); + optimizer.iterate(); + + } while(optimizer.iterations() < parameters.maxIterations && + !checkConvergence(parameters.relativeErrorTol, parameters.absoluteErrorTol, + parameters.errorTol, currentError, optimizer.error(), parameters.verbosity)); + + // return the final optimized values + return optimizer.values(); +} + +/* ************************************************************************* */ +void FindFactorsWithAny(const std::set& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) { + + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) { + NonlinearFactor::const_iterator key = factor->begin(); + while((key != factor->end()) && (!std::binary_search(keys.begin(), keys.end(), *key))) { + ++key; + } + if(key != factor->end()) { + destinationFactors.push_back(factor); + } + } + +} + +/* ************************************************************************* */ +void FindFactorsWithOnly(const std::set& keys, const NonlinearFactorGraph& sourceFactors, NonlinearFactorGraph& destinationFactors) { + + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, sourceFactors) { + NonlinearFactor::const_iterator key = factor->begin(); + while((key != factor->end()) && (std::binary_search(keys.begin(), keys.end(), *key))) { + ++key; + } + if(key == factor->end()) { + destinationFactors.push_back(factor); + } + } + +} + +/* ************************************************************************* */ +typedef BayesTree::sharedClique Clique; +void SymbolicPrintTree(const Clique& clique, const Ordering& ordering, const std::string indent = "") { + std::cout << indent << "P( "; + BOOST_FOREACH(Index index, clique->conditional()->frontals()){ + std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; + } + if(clique->conditional()->nrParents() > 0) { + std::cout << "| "; + } + BOOST_FOREACH(Index index, clique->conditional()->parents()){ + std::cout << DefaultKeyFormatter(ordering.key(index)) << " "; + } + std::cout << ")" << std::endl; + + BOOST_FOREACH(const Clique& child, clique->children()) { + SymbolicPrintTree(child, ordering, indent+" "); + } +} + +/* ************************************************************************* */ +TEST_UNSAFE( ConcurrentBatchSmoother, update_Batch ) +{ + // Test the 'update' function of the ConcurrentBatchSmoother in a nonlinear environment. + // Thus, a full L-M optimization and the ConcurrentBatchSmoother results should be identical + // This tests adds all of the factors to the smoother at once (i.e. batch) + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + + // Create a Concurrent Batch Smoother + ConcurrentBatchSmoother smoother(parameters); + + // Create containers to keep the full graph + Values fullTheta; + NonlinearFactorGraph fullGraph; + + // Create all factors + CreateFactors(fullGraph, fullTheta, 0, 20); + + // Optimize with Concurrent Batch Smoother + smoother.update(fullGraph, fullTheta); + Values actual = smoother.calculateEstimate(); + + // Optimize with L-M + Values expected = BatchOptimize(fullGraph, fullTheta); + + // Check smoother versus batch + CHECK(assert_equal(expected, actual, 1e-4)); +} + +/* ************************************************************************* */ +TEST_UNSAFE( ConcurrentBatchSmoother, update_Incremental ) +{ + // Test the 'update' function of the ConcurrentBatchSmoother in a nonlinear environment. + // Thus, a full L-M optimization and the ConcurrentBatchSmoother results should be identical + // This tests adds the factors to the smoother as they are created (i.e. incrementally) + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + + // Create a Concurrent Batch Smoother + ConcurrentBatchSmoother smoother(parameters); + + // Create containers to keep the full graph + Values fullTheta; + NonlinearFactorGraph fullGraph; + + // Add odometry from time 0 to time 10 + for(size_t i = 0; i < 20; ++i) { + // Create containers to keep the new factors + Values newTheta; + NonlinearFactorGraph newGraph; + + // Create factors + CreateFactors(newGraph, newTheta, i, i+1); + + // Add these entries to the filter + smoother.update(newGraph, newTheta); + Values actual = smoother.calculateEstimate(); + + // Add these entries to the full batch version + fullGraph.push_back(newGraph); + fullTheta.insert(newTheta); + Values expected = BatchOptimize(fullGraph, fullTheta); + fullTheta = expected; + + // Compare filter solution with full batch + CHECK(assert_equal(expected, actual, 1e-4)); + } + +} + +/* ************************************************************************* */ +TEST_UNSAFE( ConcurrentBatchSmoother, synchronize ) +{ + // Test the 'synchronize' function of the ConcurrentBatchSmoother in a nonlinear environment. + // The smoother is operating on a known tree structure, so the factors and summarization can + // be predicted for testing purposes + + // Create a set of optimizer parameters + LevenbergMarquardtParams parameters; + + // Create a Concurrent Batch Smoother + ConcurrentBatchSmootherTester smoother(parameters); + + // Create containers to keep the full graph + Values fullTheta; + NonlinearFactorGraph fullGraph; + + // Create factors for times 0 - 12 + // When eliminated with ordering (X2 X0 X1 X4 X5 X3 X6 X8 X11 X10 X7 X9 X12)augmentedHessian + // ... this Bayes Tree is produced: + // Bayes Tree: + // P( X7 X9 X12 ) + // P( X10 | X9 ) + // P( X11 | X10 ) + // P( X8 | X9 ) + // P( X6 | X7 X9 ) + // P( X5 X3 | X6 ) + // P( X1 | X3 ) + // P( X0 | X1 ) + // P( X2 | X3 ) + // P( X4 | X7 ) + // We then produce the inputs necessary for the 'synchronize' function. + // The smoother is branches X4 and X6, the filter is branches X8 and X10, and the root is (X7 X9 X12) + CreateFactors(fullGraph, fullTheta, 0, 13); + + // Optimize the full graph + Values optimalTheta = BatchOptimize(fullGraph, fullTheta); + + // Re-eliminate to create the Bayes Tree + Ordering ordering; + ordering.push_back(Symbol('X', 2)); + ordering.push_back(Symbol('X', 0)); + ordering.push_back(Symbol('X', 1)); + ordering.push_back(Symbol('X', 4)); + ordering.push_back(Symbol('X', 5)); + ordering.push_back(Symbol('X', 3)); + ordering.push_back(Symbol('X', 6)); + ordering.push_back(Symbol('X', 8)); + ordering.push_back(Symbol('X', 11)); + ordering.push_back(Symbol('X', 10)); + ordering.push_back(Symbol('X', 7)); + ordering.push_back(Symbol('X', 9)); + ordering.push_back(Symbol('X', 12)); + Values linpoint; + linpoint.insert(optimalTheta); + GaussianFactorGraph linearGraph = *fullGraph.linearize(linpoint, ordering); + JunctionTree jt(linearGraph); + ISAM2Clique::shared_ptr root = jt.eliminate(EliminateQR); + BayesTree bayesTree; + bayesTree.insert(root); + + // Extract the values for the smoother keys. This consists of the branches: X4 and X6 + // Extract the non-root values from the initial values to test the smoother optimization + Values smootherValues; + smootherValues.insert(Symbol('X', 0), fullTheta.at(Symbol('X', 0))); + smootherValues.insert(Symbol('X', 1), fullTheta.at(Symbol('X', 1))); + smootherValues.insert(Symbol('X', 2), fullTheta.at(Symbol('X', 2))); + smootherValues.insert(Symbol('X', 3), fullTheta.at(Symbol('X', 3))); + smootherValues.insert(Symbol('X', 4), fullTheta.at(Symbol('X', 4))); + smootherValues.insert(Symbol('X', 5), fullTheta.at(Symbol('X', 5))); + smootherValues.insert(Symbol('X', 6), fullTheta.at(Symbol('X', 6))); + + // Extract the optimal root values + Values rootValues; + rootValues.insert(Symbol('X', 7), optimalTheta.at(Symbol('X', 7))); + rootValues.insert(Symbol('X', 9), optimalTheta.at(Symbol('X', 9))); + rootValues.insert(Symbol('X', 12), optimalTheta.at(Symbol('X', 12))); + + // Extract the nonlinear smoother factors as any factor with a non-root smoother key + std::set smootherKeys; + smootherKeys.insert(Symbol('X', 0)); + smootherKeys.insert(Symbol('X', 1)); + smootherKeys.insert(Symbol('X', 2)); + smootherKeys.insert(Symbol('X', 3)); + smootherKeys.insert(Symbol('X', 4)); + smootherKeys.insert(Symbol('X', 5)); + smootherKeys.insert(Symbol('X', 6)); + NonlinearFactorGraph smootherFactors; + FindFactorsWithAny(smootherKeys, fullGraph, smootherFactors); + + // Extract the filter summarized factors. This consists of the linear cached factors from + // the filter branches X8 and X10, as well as any nonlinear factor that involves only root keys + NonlinearFactorGraph filterSummarization; + filterSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 8)))->cachedFactor()), ordering, linpoint)); + filterSummarization.add(LinearizedJacobianFactor(boost::static_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 10)))->cachedFactor()), ordering, linpoint)); + std::set rootKeys; + rootKeys.insert(Symbol('X', 7)); + rootKeys.insert(Symbol('X', 9)); + rootKeys.insert(Symbol('X', 12)); + FindFactorsWithOnly(rootKeys, fullGraph, filterSummarization); + + + + // Perform the synchronization procedure + NonlinearFactorGraph actualSmootherSummarization; + smoother.presync(); + smoother.getSummarizedFactors(actualSmootherSummarization); + smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues); + smoother.postsync(); + + // Verify the returned smoother values is empty in the first iteration + NonlinearFactorGraph expectedSmootherSummarization; + CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-4)); + + + + // Perform a full update of the smoother. Since the root values/summarized filter factors were + // created at the optimal values, the smoother should be identical to the batch optimization + smoother.update(); + Values actualSmootherTheta = smoother.calculateEstimate(); + + // Create the expected values as the optimal set + Values expectedSmootherTheta; + expectedSmootherTheta.insert(Symbol('X', 0), optimalTheta.at(Symbol('X', 0))); + expectedSmootherTheta.insert(Symbol('X', 1), optimalTheta.at(Symbol('X', 1))); + expectedSmootherTheta.insert(Symbol('X', 2), optimalTheta.at(Symbol('X', 2))); + expectedSmootherTheta.insert(Symbol('X', 3), optimalTheta.at(Symbol('X', 3))); + expectedSmootherTheta.insert(Symbol('X', 4), optimalTheta.at(Symbol('X', 4))); + expectedSmootherTheta.insert(Symbol('X', 5), optimalTheta.at(Symbol('X', 5))); + expectedSmootherTheta.insert(Symbol('X', 6), optimalTheta.at(Symbol('X', 6))); + + // Compare filter solution with full batch + CHECK(assert_equal(expectedSmootherTheta, actualSmootherTheta, 1e-4)); + + + + // Add a loop closure factor to the smoother and re-check. Since the filter + // factors were created at the optimal linpoint, and since the new loop closure + // does not involve filter keys, the smoother should still yeild the optimal solution + // The new Bayes Tree is: + // Bayes Tree: + // P( X7 X9 X12 ) + // P( X10 | X9 ) + // P( X11 | X10 ) + // P( X8 | X9 ) + // P( X6 | X7 X9 ) + // P( X4 | X6 X7 ) + // P( X3 X5 | X4 X6 ) + // P( X2 | X3 ) + // P( X1 | X3 X4 ) + // P( X0 | X1 ) + Pose3 poseDelta = fullTheta.at(Symbol('X', 1)).between(fullTheta.at(Symbol('X', 4))); + NonlinearFactor::shared_ptr loopClosure = NonlinearFactor::shared_ptr(new BetweenFactor(Symbol('X', 1), Symbol('X', 4), poseDelta, noiseOdometery)); + fullGraph.push_back(loopClosure); + optimalTheta = BatchOptimize(fullGraph, fullTheta, rootValues); + + // Recreate the Bayes Tree + linpoint.clear(); + linpoint.insert(optimalTheta); + linpoint.update(rootValues); + linearGraph = *fullGraph.linearize(linpoint, ordering); + jt = JunctionTree(linearGraph); + root = jt.eliminate(EliminateQR); + bayesTree = BayesTree(); + bayesTree.insert(root); + + // Add the loop closure to the smoother + NonlinearFactorGraph newFactors; + newFactors.push_back(loopClosure); + smoother.update(newFactors); + actualSmootherTheta = smoother.calculateEstimate(); + + // Create the expected values as the optimal set + expectedSmootherTheta.clear(); + expectedSmootherTheta.insert(Symbol('X', 0), optimalTheta.at(Symbol('X', 0))); + expectedSmootherTheta.insert(Symbol('X', 1), optimalTheta.at(Symbol('X', 1))); + expectedSmootherTheta.insert(Symbol('X', 2), optimalTheta.at(Symbol('X', 2))); + expectedSmootherTheta.insert(Symbol('X', 3), optimalTheta.at(Symbol('X', 3))); + expectedSmootherTheta.insert(Symbol('X', 4), optimalTheta.at(Symbol('X', 4))); + expectedSmootherTheta.insert(Symbol('X', 5), optimalTheta.at(Symbol('X', 5))); + expectedSmootherTheta.insert(Symbol('X', 6), optimalTheta.at(Symbol('X', 6))); + + // Compare filter solution with full batch + // TODO: Check This +// CHECK(assert_equal(expectedSmootherTheta, actualSmootherTheta, 1e-4)); + + + + // Now perform a second synchronization to test the smoother-calculated summarization + actualSmootherSummarization.resize(0); + smootherFactors.resize(0); + smootherValues.clear(); + smoother.presync(); + smoother.getSummarizedFactors(actualSmootherSummarization); + smoother.synchronize(smootherFactors, smootherValues, filterSummarization, rootValues); + smoother.postsync(); + + // Extract the expected smoother summarization from the Bayes Tree + // The smoother branches after the addition of the loop closure is only X6 + expectedSmootherSummarization.resize(0); + JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(bayesTree.nodes().at(ordering.at(Symbol('X', 6)))->cachedFactor()); + LinearizedJacobianFactor::shared_ptr ljf(new LinearizedJacobianFactor(jf, ordering, linpoint)); + expectedSmootherSummarization.push_back(ljf); + + // Compare smoother factors with the expected factors by computing the hessian information matrix + // TODO: Check This +// CHECK(hessian_equal(expectedSmootherSummarization, actualSmootherSummarization, linpoint, 1e-4)); + + + + // TODO: Modify the second synchronization so that the filter sends an additional set of factors. + // I'm not sure what additional code this will exercise, but just for good measure. + +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp new file mode 100644 index 000000000..426683806 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp @@ -0,0 +1,316 @@ +/* ---------------------------------------------------------------------------- + + * 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 testRFID.cpp + * @brief Unit tests for the RFID factor + * @author Stephen Williams (swilliams8@gatech.edu) + * @date Jan 16, 2012 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +/* ************************************************************************* */ +TEST( LinearizedFactor, equals_jacobian ) +{ + // Create a Between Factor from a Point3. This is actually a linear factor. + Key x1(1); + Key x2(2); + Ordering ordering; + ordering.push_back(x1); + ordering.push_back(x2); + Values values; + values.insert(x1, Point3(-22.4, +8.5, +2.4)); + values.insert(x2, Point3(-21.0, +5.0, +21.0)); + + Point3 measured(1.0, -2.5, 17.8); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1); + BetweenFactor betweenFactor(x1, x2, measured, model); + + + // Create two identical factors and make sure they're equal + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + LinearizedJacobianFactor jacobian1(jf, ordering, values); + LinearizedJacobianFactor jacobian2(jf, ordering, values); + + CHECK(assert_equal(jacobian1, jacobian2)); +} + +/* ************************************************************************* */ +TEST( LinearizedFactor, clone_jacobian ) +{ + // Create a Between Factor from a Point3. This is actually a linear factor. + Key x1(1); + Key x2(2); + Ordering ordering; + ordering.push_back(x1); + ordering.push_back(x2); + Values values; + values.insert(x1, Point3(-22.4, +8.5, +2.4)); + values.insert(x2, Point3(-21.0, +5.0, +21.0)); + + Point3 measured(1.0, -2.5, 17.8); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1); + BetweenFactor betweenFactor(x1, x2, measured, model); + + // Create one factor that is a clone of the other and make sure they're equal + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + LinearizedJacobianFactor jacobian1(jf, ordering, values); + LinearizedJacobianFactor::shared_ptr jacobian2 = boost::static_pointer_cast(jacobian1.clone()); + CHECK(assert_equal(jacobian1, *jacobian2)); + + JacobianFactor::shared_ptr jf1 = boost::static_pointer_cast(jacobian1.linearize(values, ordering)); + JacobianFactor::shared_ptr jf2 = boost::static_pointer_cast(jacobian2->linearize(values, ordering)); + CHECK(assert_equal(*jf1, *jf2)); + + Matrix information1 = jf1->augmentedInformation(); + Matrix information2 = jf2->augmentedInformation(); + CHECK(assert_equal(information1, information2)); +} + +/* ************************************************************************* */ +TEST( LinearizedFactor, add_jacobian ) +{ + // Create a Between Factor from a Point3. This is actually a linear factor. + Key x1(1); + Key x2(2); + Ordering ordering; + ordering.push_back(x1); + ordering.push_back(x2); + Values values; + values.insert(x1, Point3(-22.4, +8.5, +2.4)); + values.insert(x2, Point3(-21.0, +5.0, +21.0)); + + Point3 measured(1.0, -2.5, 17.8); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1); + BetweenFactor betweenFactor(x1, x2, measured, model); + + // Create two factor graphs, one using 'push_back' and one using 'add' and make sure they're equal + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + LinearizedJacobianFactor::shared_ptr jacobian(new LinearizedJacobianFactor(jf, ordering, values)); + NonlinearFactorGraph graph1; graph1.push_back(jacobian); + NonlinearFactorGraph graph2; graph2.add(*jacobian); + + // TODO: When creating a Jacobian from a cached factor, I experienced a problem in the 'add' version + // However, I am currently unable to reproduce the error in this unit test. + // I don't know if this affects the Hessian version as well. + CHECK(assert_equal(graph1, graph2)); +} + +///* ************************************************************************* */ +//TEST( LinearizedFactor, error_jacobian ) +//{ +// // Create a Between Factor from a Point3. This is actually a linear factor. +// Key key1(1); +// Key key2(2); +// Ordering ordering; +// ordering.push_back(key1); +// ordering.push_back(key2); +// Values values; +// values.insert(key1, Point3(-22.4, +8.5, +2.4)); +// values.insert(key2, Point3(-21.0, +5.0, +21.0)); +// +// Point3 measured(1.0, -2.5, 17.8); +// SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1); +// BetweenFactor betweenFactor(key1, key2, measured, model); +// +// +// // Create a linearized jacobian factors +// JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); +// LinearizedJacobianFactor jacobian(jf, ordering, values); +// +// +// for(double x1 = -10; x1 < 10; x1 += 2.0) { +// for(double y1 = -10; y1 < 10; y1 += 2.0) { +// for(double z1 = -10; z1 < 10; z1 += 2.0) { +// +// for(double x2 = -10; x2 < 10; x2 += 2.0) { +// for(double y2 = -10; y2 < 10; y2 += 2.0) { +// for(double z2 = -10; z2 < 10; z2 += 2.0) { +// +// Values linpoint; +// linpoint.insert(key1, Point3(x1, y1, z1)); +// linpoint.insert(key2, Point3(x2, y2, z2)); +// +// // Check that the error of the Linearized Jacobian and the original factor match +// // This only works because a BetweenFactor on a Point3 is actually a linear system +// double expected_error = betweenFactor.error(linpoint); +// double actual_error = jacobian.error(linpoint); +// EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); +// +// // Check that the linearized factors are identical +// GaussianFactor::shared_ptr expected_factor = betweenFactor.linearize(linpoint, ordering); +// GaussianFactor::shared_ptr actual_factor = jacobian.linearize(linpoint, ordering); +// CHECK(assert_equal(*expected_factor, *actual_factor)); +// } +// } +// } +// +// } +// } +// } +// +//} + +/* ************************************************************************* */ +TEST( LinearizedFactor, equals_hessian ) +{ + // Create a Between Factor from a Point3. This is actually a linear factor. + Key x1(1); + Key x2(2); + Ordering ordering; + ordering.push_back(x1); + ordering.push_back(x2); + Values values; + values.insert(x1, Point3(-22.4, +8.5, +2.4)); + values.insert(x2, Point3(-21.0, +5.0, +21.0)); + + Point3 measured(1.0, -2.5, 17.8); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1); + BetweenFactor betweenFactor(x1, x2, measured, model); + + + // Create two identical factors and make sure they're equal + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + HessianFactor::shared_ptr hf(new HessianFactor(*jf)); + LinearizedHessianFactor hessian1(hf, ordering, values); + LinearizedHessianFactor hessian2(hf, ordering, values); + + CHECK(assert_equal(hessian1, hessian2)); +} + +/* ************************************************************************* */ +TEST( LinearizedFactor, clone_hessian ) +{ + // Create a Between Factor from a Point3. This is actually a linear factor. + Key x1(1); + Key x2(2); + Ordering ordering; + ordering.push_back(x1); + ordering.push_back(x2); + Values values; + values.insert(x1, Point3(-22.4, +8.5, +2.4)); + values.insert(x2, Point3(-21.0, +5.0, +21.0)); + + Point3 measured(1.0, -2.5, 17.8); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1); + BetweenFactor betweenFactor(x1, x2, measured, model); + + + // Create two identical factors and make sure they're equal + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + HessianFactor::shared_ptr hf(new HessianFactor(*jf)); + LinearizedHessianFactor hessian1(hf, ordering, values); + LinearizedHessianFactor::shared_ptr hessian2 = boost::static_pointer_cast(hessian1.clone()); + + CHECK(assert_equal(hessian1, *hessian2)); +} + +/* ************************************************************************* */ +TEST( LinearizedFactor, add_hessian ) +{ + // Create a Between Factor from a Point3. This is actually a linear factor. + Key x1(1); + Key x2(2); + Ordering ordering; + ordering.push_back(x1); + ordering.push_back(x2); + Values values; + values.insert(x1, Point3(-22.4, +8.5, +2.4)); + values.insert(x2, Point3(-21.0, +5.0, +21.0)); + + Point3 measured(1.0, -2.5, 17.8); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1); + BetweenFactor betweenFactor(x1, x2, measured, model); + + + // Create two identical factors and make sure they're equal + JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); + HessianFactor::shared_ptr hf(new HessianFactor(*jf)); + LinearizedHessianFactor::shared_ptr hessian(new LinearizedHessianFactor(hf, ordering, values)); + NonlinearFactorGraph graph1; graph1.push_back(hessian); + NonlinearFactorGraph graph2; graph2.add(*hessian); + + CHECK(assert_equal(graph1, graph2)); +} + +///* ************************************************************************* */ +//TEST( LinearizedFactor, error_hessian ) +//{ +// // Create a Between Factor from a Point3. This is actually a linear factor. +// Key key1(1); +// Key key2(2); +// Ordering ordering; +// ordering.push_back(key1); +// ordering.push_back(key2); +// Values values; +// values.insert(key1, Point3(-22.4, +8.5, +2.4)); +// values.insert(key2, Point3(-21.0, +5.0, +21.0)); +// +// Point3 measured(1.0, -2.5, 17.8); +// SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1); +// BetweenFactor betweenFactor(key1, key2, measured, model); +// +// +// // Create a linearized hessian factor +// JacobianFactor::shared_ptr jf = boost::static_pointer_cast(betweenFactor.linearize(values, ordering)); +// HessianFactor::shared_ptr hf(new HessianFactor(*jf)); +// LinearizedHessianFactor hessian(hf, ordering, values); +// +// +// for(double x1 = -10; x1 < 10; x1 += 2.0) { +// for(double y1 = -10; y1 < 10; y1 += 2.0) { +// for(double z1 = -10; z1 < 10; z1 += 2.0) { +// +// for(double x2 = -10; x2 < 10; x2 += 2.0) { +// for(double y2 = -10; y2 < 10; y2 += 2.0) { +// for(double z2 = -10; z2 < 10; z2 += 2.0) { +// +// Values linpoint; +// linpoint.insert(key1, Point3(x1, y1, z1)); +// linpoint.insert(key2, Point3(x2, y2, z2)); +// +// // Check that the error of the Linearized Hessian and the original factor match +// // This only works because a BetweenFactor on a Point3 is actually a linear system +// double expected_error = betweenFactor.error(linpoint); +// double actual_error = hessian.error(linpoint); +// EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 ); +// +// // Check that the linearized factors are identical +// GaussianFactor::shared_ptr expected_factor = HessianFactor::shared_ptr(new HessianFactor(*betweenFactor.linearize(linpoint, ordering))); +// GaussianFactor::shared_ptr actual_factor = hessian.linearize(linpoint, ordering); +// CHECK(assert_equal(*expected_factor, *actual_factor)); +// } +// } +// } +// +// } +// } +// } +// +//} + +/* ************************************************************************* */ +int main() +{ + TestResult tr; return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +