12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged in fix/matlab_wrapper (pull request #235) 

MATLAB wrapper
release/4.3a0
Frank Dellaert 2016-02-26 09:18:07 -08:00
parent 7c351bb4e0 853b5192a5
commit b56ecd6a42
13 changed files with 79 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam.h
View File

@ -2161,6 +2161,8 @@ class ISAM2Result {
size_t getCliques() const;
};
class FactorIndices {};
class ISAM2 {
ISAM2();
ISAM2(const gtsam::ISAM2Params& params);
@ -2173,8 +2175,8 @@ class ISAM2 {
gtsam::ISAM2Result update();
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys);
// TODO: wrap the full version of update
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys);
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys, bool force_relinearize);

2
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -46,7 +46,7 @@ void ISAM2::Impl::AddVariables(
/* ************************************************************************* */
void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots,
NonlinearFactorGraph& nonlinearFactors, FastVector<size_t>& newFactorIndices)
NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices)
{
newFactorIndices.resize(newFactors.size());

2
gtsam/nonlinear/ISAM2-impl.h
View File

@ -52,7 +52,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
/// complete list of nonlinear factors, and populates the list of new factor indices, both
/// optionally finding and reusing empty factor slots.
static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots,
NonlinearFactorGraph& nonlinearFactors, FastVector<size_t>& newFactorIndices);
NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices);
/**
* Remove variables from the ISAM2 system.

6
gtsam/nonlinear/ISAM2.cpp
View File

@ -510,7 +510,7 @@ boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const Ke
/* ************************************************************************* */
ISAM2Result ISAM2::update(
const NonlinearFactorGraph& newFactors, const Values& newTheta, const vector<size_t>& removeFactorIndices,
const NonlinearFactorGraph& newFactors, const Values& newTheta, const FactorIndices& removeFactorIndices,
const boost::optional<FastMap<Key,int> >& constrainedKeys, const boost::optional<FastList<Key> >& noRelinKeys,
const boost::optional<FastList<Key> >& extraReelimKeys, bool force_relinearize)
{
@ -753,8 +753,8 @@ ISAM2Result ISAM2::update(
/* ************************************************************************* */
void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
boost::optional<std::vector<size_t>&> marginalFactorsIndices,
boost::optional<std::vector<size_t>&> deletedFactorsIndices)
boost::optional<FactorIndices&> marginalFactorsIndices,
boost::optional<FactorIndices&> deletedFactorsIndices)
{
// Convert to ordered set
KeySet leafKeys(leafKeysList.begin(), leafKeysList.end());

9
gtsam/nonlinear/ISAM2.h
View File

@ -257,6 +257,7 @@ struct GTSAM_EXPORT ISAM2Params {
/// @}
};
typedef FastVector<size_t> FactorIndices;
/**
* @addtogroup ISAM2
@ -318,7 +319,7 @@ struct GTSAM_EXPORT ISAM2Result {
* factors passed as \c newFactors to ISAM2::update(). These indices may be
* used later to refer to the factors in order to remove them.
*/
FastVector<size_t> newFactorsIndices;
FactorIndices newFactorsIndices;
/** A struct holding detailed results, which must be enabled with
* ISAM2Params::enableDetailedResults.
@ -530,7 +531,7 @@ public:
*/
virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
const Values& newTheta = Values(),
const std::vector<size_t>& removeFactorIndices = std::vector<size_t>(),
const FactorIndices& removeFactorIndices = FactorIndices(),
const boost::optional<FastMap<Key,int> >& constrainedKeys = boost::none,
const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
@ -551,8 +552,8 @@ public:
* indices of any factor that was removed during the 'marginalizeLeaves' call
*/
void marginalizeLeaves(const FastList<Key>& leafKeys,
boost::optional<std::vector<size_t>&> marginalFactorsIndices = boost::none,
boost::optional<std::vector<size_t>&> deletedFactorsIndices = boost::none);
boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);
/// Access the current linearization point
const Values& getLinearizationPoint() const {

34
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -43,7 +43,7 @@ bool ConcurrentIncrementalFilter::equals(const ConcurrentFilter& rhs, double tol
/* ************************************************************************* */
ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta,
const boost::optional<FastList<Key> >& keysToMove, const boost::optional< std::vector<size_t> >& removeFactorIndices) {
const boost::optional<FastList<Key> >& keysToMove, const boost::optional< FactorIndices >& removeFactorIndices) {
gttic(update);
@ -56,43 +56,43 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
Result result;
// We do not need to remove any factors at this time
gtsam::FastVector<size_t> removedFactors;
FactorIndices removedFactors;
if(removeFactorIndices){
removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end());
}
// Generate ordering constraints that force the 'keys to move' to the end
boost::optional<gtsam::FastMap<gtsam::Key,int> > orderingConstraints = boost::none;
boost::optional<FastMap<Key,int> > orderingConstraints = boost::none;
if(keysToMove && keysToMove->size() > 0) {
orderingConstraints = gtsam::FastMap<gtsam::Key,int>();
orderingConstraints = FastMap<Key,int>();
int group = 1;
// Set all existing variables to Group1
if(isam2_.getLinearizationPoint().size() > 0) {
BOOST_FOREACH(const gtsam::Values::ConstKeyValuePair& key_value, isam2_.getLinearizationPoint()) {
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, isam2_.getLinearizationPoint()) {
orderingConstraints->operator[](key_value.key) = group;
}
++group;
}
// Assign new variables to the root
BOOST_FOREACH(const gtsam::Values::ConstKeyValuePair& key_value, newTheta) {
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
orderingConstraints->operator[](key_value.key) = group;
}
// Set marginalizable variables to Group0
BOOST_FOREACH(gtsam::Key key, *keysToMove){
BOOST_FOREACH(Key key, *keysToMove){
orderingConstraints->operator[](key) = 0;
}
}
// Create the set of linear keys that iSAM2 should hold constant
// iSAM2 takes care of this for us; no need to specify additional noRelin keys
boost::optional<gtsam::FastList<gtsam::Key> > noRelinKeys = boost::none;
boost::optional<FastList<Key> > noRelinKeys = boost::none;
// Mark additional keys between the 'keys to move' and the leaves
boost::optional<FastList<Key> > additionalKeys = boost::none;
if(keysToMove && keysToMove->size() > 0) {
std::set<Key> markedKeys;
BOOST_FOREACH(gtsam::Key key, *keysToMove) {
BOOST_FOREACH(Key key, *keysToMove) {
if(isam2_.getLinearizationPoint().exists(key)) {
ISAM2Clique::shared_ptr clique = isam2_[key];
GaussianConditional::const_iterator key_iter = clique->conditional()->begin();
@ -100,7 +100,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
markedKeys.insert(*key_iter);
++key_iter;
}
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) {
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
RecursiveMarkAffectedKeys(key, child, markedKeys);
}
}
@ -110,14 +110,14 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
// Update the system using iSAM2
gttic(isam2);
gtsam::ISAM2Result isam2Result = isam2_.update(newFactors, newTheta, removedFactors, orderingConstraints, noRelinKeys, additionalKeys);
ISAM2Result isam2Result = isam2_.update(newFactors, newTheta, removedFactors, orderingConstraints, noRelinKeys, additionalKeys);
gttoc(isam2);
if(keysToMove && keysToMove->size() > 0) {
gttic(cache_smoother_factors);
// Find the set of factors that will be removed
std::vector<size_t> removedFactorSlots = FindAdjacentFactors(isam2_, *keysToMove, currentSmootherSummarizationSlots_);
FactorIndices removedFactorSlots = FindAdjacentFactors(isam2_, *keysToMove, currentSmootherSummarizationSlots_);
// Cache these factors for later transmission to the smoother
NonlinearFactorGraph removedFactors;
BOOST_FOREACH(size_t slot, removedFactorSlots) {
@ -134,8 +134,8 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
gttoc(cache_smoother_factors);
gttic(marginalize);
std::vector<size_t> marginalFactorsIndices;
std::vector<size_t> deletedFactorsIndices;
FactorIndices marginalFactorsIndices;
FactorIndices deletedFactorsIndices;
isam2_.marginalizeLeaves(*keysToMove, marginalFactorsIndices, deletedFactorsIndices);
currentSmootherSummarizationSlots_.insert(currentSmootherSummarizationSlots_.end(), marginalFactorsIndices.begin(), marginalFactorsIndices.end());
BOOST_FOREACH(size_t index, deletedFactorsIndices) {
@ -210,7 +210,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth
isam2_.params().getEliminationFunction());
// Remove the old factors on the separator and insert the new ones
FastVector<size_t> removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end());
FactorIndices removeFactors(currentSmootherSummarizationSlots_.begin(), currentSmootherSummarizationSlots_.end());
ISAM2Result result = isam2_.update(currentSmootherSummarization, Values(), removeFactors, boost::none, noRelinKeys, boost::none, false);
currentSmootherSummarizationSlots_ = result.newFactorsIndices;
@ -285,10 +285,10 @@ void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(const Key& key, cons
}
/* ************************************************************************* */
std::vector<size_t> ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const std::vector<size_t>& factorsToIgnore) {
FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const FactorIndices& factorsToIgnore) {
// Identify any new factors to be sent to the smoother (i.e. any factor involving keysToMove)
std::vector<size_t> removedFactorSlots;
FactorIndices removedFactorSlots;
const VariableIndex& variableIndex = isam2.getVariableIndex();
BOOST_FOREACH(Key key, keys) {
const FastVector<size_t>& slots = variableIndex[key];

8
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h
View File

@ -46,7 +46,7 @@ public:
* factors passed as \c newFactors update(). These indices may be
* used later to refer to the factors in order to remove them.
*/
std::vector<size_t> newFactorsIndices;
FactorIndices newFactorsIndices;
double error; ///< The final factor graph error
@ -124,7 +124,7 @@ public:
*/
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
const boost::optional<FastList<Key> >& keysToMove = boost::none,
const boost::optional< std::vector<size_t> >& removeFactorIndices = boost::none);
const boost::optional< FactorIndices >& removeFactorIndices = boost::none);
/**
* Perform any required operations before the synchronization process starts.
@ -170,7 +170,7 @@ protected:
// ???
NonlinearFactorGraph previousSmootherSummarization_; ///< The smoother summarization on the old separator sent by the smoother during the last synchronization
std::vector<size_t> currentSmootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator
FactorIndices currentSmootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator
NonlinearFactorGraph smootherShortcut_; ///< A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update)
// Storage for information to be sent to the smoother
@ -183,7 +183,7 @@ private:
static void RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys);
/** Find the set of iSAM2 factors adjacent to 'keys' */
static std::vector<size_t> FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const std::vector<size_t>& factorsToIgnore);
static FactorIndices FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const FactorIndices& factorsToIgnore);
/** Update the shortcut marginal between the current separator keys and the previous separator keys */
// TODO: Make this a static function

10
gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
View File

@ -45,14 +45,14 @@ bool ConcurrentIncrementalSmoother::equals(const ConcurrentSmoother& rhs, double
/* ************************************************************************* */
ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta,
const boost::optional< std::vector<size_t> >& removeFactorIndices) {
const boost::optional<FactorIndices>& removeFactorIndices) {
gttic(update);
// Create the return result meta-data
Result result;
gtsam::FastVector<size_t> removedFactors;
FastVector<size_t> removedFactors;
if(removeFactorIndices){
// Be very careful to this line
@ -72,7 +72,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons
}
// Use iSAM2 to perform an update
gtsam::ISAM2Result isam2Result;
ISAM2Result isam2Result;
if(isam2_.getFactorsUnsafe().size() + newFactors.size() + smootherFactors_.size() + filterSummarizationFactors_.size() > 0) {
if(synchronizationUpdatesAvailable_) {
// Augment any new factors/values with the cached data from the last synchronization
@ -106,7 +106,7 @@ ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmoother::update(cons
synchronizationUpdatesAvailable_ = false;
} else {
// Update the system using iSAM2
isam2Result = isam2_.update(newFactors, newTheta, FastVector<size_t>(), constrainedKeys, noRelinKeys);
isam2Result = isam2_.update(newFactors, newTheta, FactorIndices(), constrainedKeys, noRelinKeys);
}
}
@ -245,7 +245,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
}
// Get the set of separator keys
gtsam::KeySet separatorKeys;
KeySet separatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
separatorKeys.insert(key_value.key);
}

4
gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h
View File

@ -109,7 +109,7 @@ public:
* and additionally, variables that were already in the system must not be included here.
*/
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
const boost::optional< std::vector<size_t> >& removeFactorIndices = boost::none);
const boost::optional<FactorIndices>& removeFactorIndices = boost::none);
/**
* Perform any required operations before the synchronization process starts.
@ -152,7 +152,7 @@ protected:
Values smootherValues_; ///< New variables to be added to the smoother during the next update
NonlinearFactorGraph filterSummarizationFactors_; ///< New filter summarization factors to replace the existing filter summarization during the next update
Values separatorValues_; ///< The linearization points of the separator variables. These should not be changed during optimization.
FastVector<size_t> filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors
FactorIndices filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors
bool synchronizationUpdatesAvailable_; ///< Flag indicating the currently stored synchronization updates have not been applied yet
// Storage for information to be sent to the filter

32
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
View File

@ -115,10 +115,10 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
}
// Mark additional keys between the marginalized keys and the leaves
std::set<gtsam::Key> additionalKeys;
BOOST_FOREACH(gtsam::Key key, marginalizableKeys) {
gtsam::ISAM2Clique::shared_ptr clique = isam_[key];
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) {
std::set<Key> additionalKeys;
BOOST_FOREACH(Key key, marginalizableKeys) {
ISAM2Clique::shared_ptr clique = isam_[key];
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
recursiveMarkAffectedKeys(key, child, additionalKeys);
}
}
@ -126,7 +126,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
// Update iSAM2
ISAM2Result isamResult = isam_.update(newFactors, newTheta,
FastVector<size_t>(), constrainedKeys, boost::none, additionalMarkedKeys);
FactorIndices(), constrainedKeys, boost::none, additionalMarkedKeys);
if (debug) {
PrintSymbolicTree(isam_,
@ -194,8 +194,8 @@ void IncrementalFixedLagSmoother::createOrderingConstraints(
void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
const std::string& label) {
std::cout << label;
BOOST_FOREACH(gtsam::Key key, keys) {
std::cout << " " << gtsam::DefaultKeyFormatter(key);
BOOST_FOREACH(Key key, keys) {
std::cout << " " << DefaultKeyFormatter(key);
}
std::cout << std::endl;
}
@ -204,8 +204,8 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
void IncrementalFixedLagSmoother::PrintSymbolicFactor(
const GaussianFactor::shared_ptr& factor) {
std::cout << "f(";
BOOST_FOREACH(gtsam::Key key, factor->keys()) {
std::cout << " " << gtsam::DefaultKeyFormatter(key);
BOOST_FOREACH(Key key, factor->keys()) {
std::cout << " " << DefaultKeyFormatter(key);
}
std::cout << " )" << std::endl;
}
@ -220,7 +220,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicGraph(
}
/* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam,
void IncrementalFixedLagSmoother::PrintSymbolicTree(const ISAM2& isam,
const std::string& label) {
std::cout << label << std::endl;
if (!isam.roots().empty()) {
@ -233,22 +233,22 @@ void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam,
/* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(
const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent) {
const ISAM2Clique::shared_ptr& clique, const std::string indent) {
// Print the current clique
std::cout << indent << "P( ";
BOOST_FOREACH(gtsam::Key key, clique->conditional()->frontals()) {
std::cout << gtsam::DefaultKeyFormatter(key) << " ";
BOOST_FOREACH(Key key, clique->conditional()->frontals()) {
std::cout << DefaultKeyFormatter(key) << " ";
}
if (clique->conditional()->nrParents() > 0)
std::cout << "| ";
BOOST_FOREACH(gtsam::Key key, clique->conditional()->parents()) {
std::cout << gtsam::DefaultKeyFormatter(key) << " ";
BOOST_FOREACH(Key key, clique->conditional()->parents()) {
std::cout << DefaultKeyFormatter(key) << " ";
}
std::cout << ")" << std::endl;
// Recursively print all of the children
BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children) {
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) {
PrintSymbolicTreeHelper(child, indent + " ");
}
}

30
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
View File

@ -559,7 +559,6 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
/* ************************************************************************* */
TEST( ConcurrentIncrementalFilter, synchronize_0 )
{
std::cout << "*********************** synchronize_0 ************************" << std::endl;
// Create a set of optimizer parameters
ISAM2Params parameters;
@ -593,7 +592,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_0 )
///* ************************************************************************* */
TEST( ConcurrentIncrementalFilter, synchronize_1 )
{
std::cout << "*********************** synchronize_1 ************************" << std::endl;
// Create a set of optimizer parameters
ISAM2Params parameters;
parameters.relinearizeThreshold = 0;
@ -641,7 +639,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_1 )
/* ************************************************************************* */
TEST( ConcurrentIncrementalFilter, synchronize_2 )
{
std::cout << "*********************** synchronize_2 ************************" << std::endl;
// Create a set of optimizer parameters
ISAM2Params parameters;
parameters.relinearizeThreshold = 0;
@ -712,7 +709,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_2 )
/* ************************************************************************* */
TEST( ConcurrentIncrementalFilter, synchronize_3 )
{
std::cout << "*********************** synchronize_3 ************************" << std::endl;
// Create a set of optimizer parameters
ISAM2Params parameters;
parameters.relinearizeThreshold = 0;
@ -800,7 +796,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_3 )
/* ************************************************************************* */
TEST( ConcurrentIncrementalFilter, synchronize_4 )
{
std::cout << "*********************** synchronize_4 ************************" << std::endl;
// Create a set of optimizer parameters
ISAM2Params parameters;
parameters.relinearizeThreshold = 0;
@ -896,7 +891,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_4 )
/* ************************************************************************* */
TEST( ConcurrentIncrementalFilter, synchronize_5 )
{
std::cout << "*********************** synchronize_5 ************************" << std::endl;
// Create a set of optimizer parameters
ISAM2Params parameters;
parameters.relinearizeThreshold = 0;
@ -1092,7 +1086,6 @@ TEST( ConcurrentIncrementalFilter, synchronize_5 )
///* ************************************************************************* */
TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
{
std::cout << "*********************** CalculateMarginals_1 ************************" << std::endl;
// We compare the manual computation of the linear marginals from a factor graph, with the function CalculateMarginals
NonlinearFactor::shared_ptr factor1(new PriorFactor<Pose3>(1, poseInitial, noisePrior));
NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
@ -1141,8 +1134,6 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
///* ************************************************************************* */
TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
{
std::cout << "*********************** CalculateMarginals_2 ************************" << std::endl;
// We compare the manual computation of the linear marginals from a factor graph, with the function CalculateMarginals
NonlinearFactor::shared_ptr factor1(new PriorFactor<Pose3>(1, poseInitial, noisePrior));
NonlinearFactor::shared_ptr factor2(new BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
@ -1165,7 +1156,6 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
newValues.insert(3, value3);
// Create the set of marginalizable variables
std::vector<Key> linearIndices;
linearIndices.push_back(1);
@ -1190,8 +1180,6 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
// Check
CHECK(assert_equal(expectedMarginals, actualMarginals, 1e-6));
// actualMarginals.print("actualMarginals \n");
// expectedMarginals.print("expectedMarginals \n");
}
@ -1199,8 +1187,6 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
///* ************************************************************************* */
TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 )
{
std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl;
// Create a set of optimizer parameters
ISAM2Params parameters;
parameters.relinearizeThreshold = 0;
@ -1233,8 +1219,9 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 )
// factor we want to remove
// NOTE: we can remove factors, paying attention that the remaining graph remains connected
// we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> removeFactorIndices(1,1);
// we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery)
FactorIndices removeFactorIndices;
removeFactorIndices.push_back(1);
// Add no factors to the filter (we only want to test the removal)
NonlinearFactorGraph noFactors;
@ -1245,7 +1232,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 )
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery))
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
@ -1258,7 +1245,6 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 )
/////* ************************************************************************* */
TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 )
{
std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl;
// we try removing the last factor
ISAM2Params parameters;
@ -1293,7 +1279,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 )
// factor we want to remove
// NOTE: we can remove factors, paying attention that the remaining graph remains connected
// we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> removeFactorIndices(1,4);
FactorIndices removeFactorIndices(1,4);
// Add no factors to the filter (we only want to test the removal)
NonlinearFactorGraph noFactors;
@ -1318,7 +1304,6 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 )
/////* ************************************************************************* */
TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 )
{
std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl;
// we try removing the first factor
ISAM2Params parameters;
@ -1353,7 +1338,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 )
// factor we want to remove
// NOTE: we can remove factors, paying attention that the remaining graph remains connected
// we remove a single factor, the number 0, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> removeFactorIndices(1,0);
FactorIndices removeFactorIndices(1,0);
// Add no factors to the filter (we only want to test the removal)
NonlinearFactorGraph noFactors;
@ -1376,7 +1361,6 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 )
/////* ************************************************************************* */
TEST( ConcurrentIncrementalFilter, removeFactors_values )
{
std::cout << "*********************** removeFactors_values ************************" << std::endl;
// we try removing the last factor
ISAM2Params parameters;
@ -1411,7 +1395,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values )
// factor we want to remove
// NOTE: we can remove factors, paying attention that the remaining graph remains connected
// we remove a single factor, the number 4, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> removeFactorIndices(1,4);
FactorIndices removeFactorIndices(1,4);
// Add no factors to the filter (we only want to test the removal)
NonlinearFactorGraph noFactors;

9
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
View File

@ -600,8 +600,6 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
///* ************************************************************************* */
TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 )
{
std::cout << "*********************** removeFactors_topology_1 ************************" << std::endl;
// Create a set of optimizer parameters
ISAM2Params parameters;
parameters.optimizationParams = ISAM2GaussNewtonParams();
@ -629,9 +627,7 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 )
// factor we want to remove
// NOTE: we can remove factors, paying attention that the remaining graph remains connected
// we remove a single factor, the number 1, which is a BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery);
std::vector<size_t> removeFactorIndices(2,1);
FactorIndices removeFactorIndices(2,1);
// Add no factors to the smoother (we only want to test the removal)
NonlinearFactorGraph noFactors;
@ -657,7 +653,6 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 )
/////* ************************************************************************* */
//TEST( ConcurrentIncrementalSmoother, removeFactors_topology_2 )
//{
// std::cout << "*********************** removeFactors_topology_2 ************************" << std::endl;
// // we try removing the last factor
//
// // Create a set of optimizer parameters
@ -711,7 +706,6 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 )
/////* ************************************************************************* */
//TEST( ConcurrentBatchSmoother, removeFactors_topology_3 )
//{
// std::cout << "*********************** removeFactors_topology_3 ************************" << std::endl;
// // we try removing the first factor
//
// // Create a set of optimizer parameters
@ -761,7 +755,6 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 )
/////* ************************************************************************* */
//TEST( ConcurrentBatchSmoother, removeFactors_values )
//{
// std::cout << "*********************** removeFactors_values ************************" << std::endl;
// // we try removing the last factor
//
// // Create a set of optimizer parameters

26
tests/testGaussianISAM2.cpp
View File

@ -301,9 +301,9 @@ TEST(ISAM2, AddFactorsStep1)
expectedNonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);
expectedNonlinearFactors += PriorFactor<LieScalar>(2, Zero, model);
const FastVector<size_t> expectedNewFactorIndices = list_of(1)(3);
const FactorIndices expectedNewFactorIndices = list_of(1)(3);
FastVector<size_t> actualNewFactorIndices;
FactorIndices actualNewFactorIndices;
ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices);
@ -419,7 +419,7 @@ TEST(ISAM2, removeFactors)
ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
// Remove the 2nd measurement on landmark 0 (Key 100)
FastVector<size_t> toRemove;
FactorIndices toRemove;
toRemove.push_back(12);
isam.update(NonlinearFactorGraph(), Values(), toRemove);
@ -439,7 +439,7 @@ TEST(ISAM2, removeVariables)
ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
// Remove the measurement on landmark 0 (Key 100)
FastVector<size_t> toRemove;
FactorIndices toRemove;
toRemove.push_back(7);
toRemove.push_back(14);
isam.update(NonlinearFactorGraph(), Values(), toRemove);
@ -466,7 +466,7 @@ TEST(ISAM2, swapFactors)
// Remove the measurement on landmark 0 and replace with a different one
{
size_t swap_idx = isam.getFactorsUnsafe().size()-2;
FastVector<size_t> toRemove;
FactorIndices toRemove;
toRemove.push_back(swap_idx);
fullgraph.remove(swap_idx);
@ -549,7 +549,7 @@ TEST(ISAM2, constrained_ordering)
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
if(i >= 3)
isam.update(newfactors, init, FastVector<size_t>(), constrained);
isam.update(newfactors, init, FactorIndices(), constrained);
else
isam.update(newfactors, init);
}
@ -570,7 +570,7 @@ TEST(ISAM2, constrained_ordering)
fullinit.insert(100, Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0)));
fullinit.insert(101, Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0)));
isam.update(newfactors, init, FastVector<size_t>(), constrained);
isam.update(newfactors, init, FactorIndices(), constrained);
++ i;
}
@ -584,7 +584,7 @@ TEST(ISAM2, constrained_ordering)
init.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01));
isam.update(newfactors, init, FastVector<size_t>(), constrained);
isam.update(newfactors, init, FactorIndices(), constrained);
}
// Add odometry from time 10 to 11 and landmark measurement at time 10
@ -599,7 +599,7 @@ TEST(ISAM2, constrained_ordering)
init.insert((i+1), Pose2(6.9, 0.1, 0.01));
fullinit.insert((i+1), Pose2(6.9, 0.1, 0.01));
isam.update(newfactors, init, FastVector<size_t>(), constrained);
isam.update(newfactors, init, FactorIndices(), constrained);
++ i;
}
@ -713,7 +713,7 @@ TEST(ISAM2, marginalizeLeaves1)
constrainedKeys.insert(make_pair(1,1));
constrainedKeys.insert(make_pair(2,2));
isam.update(factors, values, FastVector<size_t>(), constrainedKeys);
isam.update(factors, values, FactorIndices(), constrainedKeys);
FastList<Key> leafKeys = list_of(0);
EXPECT(checkMarginalizeLeaves(isam, leafKeys));
@ -744,7 +744,7 @@ TEST(ISAM2, marginalizeLeaves2)
constrainedKeys.insert(make_pair(2,2));
constrainedKeys.insert(make_pair(3,3));
isam.update(factors, values, FastVector<size_t>(), constrainedKeys);
isam.update(factors, values, FactorIndices(), constrainedKeys);
FastList<Key> leafKeys = list_of(0);
EXPECT(checkMarginalizeLeaves(isam, leafKeys));
@ -784,7 +784,7 @@ TEST(ISAM2, marginalizeLeaves3)
constrainedKeys.insert(make_pair(4,4));
constrainedKeys.insert(make_pair(5,5));
isam.update(factors, values, FastVector<size_t>(), constrainedKeys);
isam.update(factors, values, FactorIndices(), constrainedKeys);
FastList<Key> leafKeys = list_of(0);
EXPECT(checkMarginalizeLeaves(isam, leafKeys));
@ -810,7 +810,7 @@ TEST(ISAM2, marginalizeLeaves4)
constrainedKeys.insert(make_pair(1,1));
constrainedKeys.insert(make_pair(2,2));
isam.update(factors, values, FastVector<size_t>(), constrainedKeys);
isam.update(factors, values, FactorIndices(), constrainedKeys);
FastList<Key> leafKeys = list_of(1);
EXPECT(checkMarginalizeLeaves(isam, leafKeys));