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

Merge remote-tracking branch 'origin/develop' into feature/SmartFactors3 

# Conflicts:
#	gtsam/linear/tests/testRegularHessianFactor.cpp
#	gtsam/slam/SmartFactorBase.h
release/4.3a0
cbeall3 2015-06-23 10:03:42 -04:00
parent 3a264ecc8c b7b7182148
commit b0f04503c9
74 changed files with 695 additions and 542 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
CMakeLists.txt
View File

@ -179,6 +179,11 @@ if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL
include_directories(${MKL_INCLUDE_DIR})
list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES})
# --no-as-needed is required with gcc according to the MKL link advisor
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed")
endif()
else()
set(GTSAM_USE_EIGEN_MKL 0)
set(EIGEN_USE_MKL_ALL 0)

1
examples/SolverComparer.cpp
View File

@ -44,6 +44,7 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>

1
examples/TimeTBB.cpp
View File

@ -17,6 +17,7 @@
#include <gtsam/global_includes.h>
#include <gtsam/base/Matrix.h>
#include <boost/assign/list_of.hpp>
#include <boost/foreach.hpp>
#include <map>

123
gtsam/base/FastSet.h
View File

@ -18,26 +18,22 @@
#pragma once
#include <gtsam/base/FastDefaultAllocator.h>
#include <boost/mpl/has_xxx.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/set.hpp>
#include <set>
#include <iostream>
#include <string>
#include <cmath>
#include <gtsam/base/FastDefaultAllocator.h>
#include <gtsam/base/Testable.h>
BOOST_MPL_HAS_XXX_TRAIT_DEF(print)
#include <functional>
#include <set>
namespace boost {
namespace serialization {
class access;
} /* namespace serialization */
} /* namespace boost */
namespace gtsam {
// This is used internally to allow this container to be Testable even when it
// contains non-testable elements.
template<typename VALUE, class ENABLE = void>
struct FastSetTestableHelper;
/**
* FastSet is a thin wrapper around std::set that uses the boost
* fast_pool_allocator instead of the default STL allocator. This is just a
@ -45,12 +41,16 @@ struct FastSetTestableHelper;
* we've seen that the fast_pool_allocator can lead to speedups of several %.
* @addtogroup base
*/
template<typename VALUE, class ENABLE = void>
class FastSet: public std::set<VALUE, std::less<VALUE>, typename internal::FastDefaultAllocator<VALUE>::type> {
template<typename VALUE>
class FastSet: public std::set<VALUE, std::less<VALUE>,
typename internal::FastDefaultAllocator<VALUE>::type> {
BOOST_CONCEPT_ASSERT ((IsTestable<VALUE> ));
public:
typedef std::set<VALUE, std::less<VALUE>, typename internal::FastDefaultAllocator<VALUE>::type> Base;
typedef std::set<VALUE, std::less<VALUE>,
typename internal::FastDefaultAllocator<VALUE>::type> Base;
/** Default constructor */
FastSet() {
@ -59,23 +59,23 @@ public:
/** Constructor from a range, passes through to base class */
template<typename INPUTITERATOR>
explicit FastSet(INPUTITERATOR first, INPUTITERATOR last) :
Base(first, last) {
Base(first, last) {
}
/** Constructor from a iterable container, passes through to base class */
template<typename INPUTCONTAINER>
explicit FastSet(const INPUTCONTAINER& container) :
Base(container.begin(), container.end()) {
Base(container.begin(), container.end()) {
}
/** Copy constructor from another FastSet */
FastSet(const FastSet<VALUE>& x) :
Base(x) {
Base(x) {
}
/** Copy constructor from the base set class */
FastSet(const Base& x) :
Base(x) {
Base(x) {
}
#ifdef GTSAM_ALLOCATOR_BOOSTPOOL
@ -85,7 +85,7 @@ public:
// STL vector where if the size is zero, the pool allocator will allocate
// huge amounts of memory.
if(x.size() > 0)
Base::insert(x.begin(), x.end());
Base::insert(x.begin(), x.end());
}
#endif
@ -95,17 +95,31 @@ public:
}
/** Handy 'exists' function */
bool exists(const VALUE& e) const { return this->find(e) != this->end(); }
bool exists(const VALUE& e) const {
return this->find(e) != this->end();
}
/** Print to implement Testable */
void print(const std::string& str = "") const { FastSetTestableHelper<VALUE>::print(*this, str); }
/** Print to implement Testable: pretty basic */
void print(const std::string& str = "") const {
for (typename Base::const_iterator it = this->begin(); it != this->end(); ++it)
traits<VALUE>::Print(*it, str);
}
/** Check for equality within tolerance to implement Testable */
bool equals(const FastSet<VALUE>& other, double tol = 1e-9) const { return FastSetTestableHelper<VALUE>::equals(*this, other, tol); }
bool equals(const FastSet<VALUE>& other, double tol = 1e-9) const {
typename Base::const_iterator it1 = this->begin(), it2 = other.begin();
while (it1 != this->end()) {
if (it2 == other.end() || !traits<VALUE>::Equals(*it2, *it2, tol))
return false;
++it1;
++it2;
}
return true;
}
/** insert another set: handy for MATLAB access */
void merge(const FastSet& other) {
Base::insert(other.begin(),other.end());
Base::insert(other.begin(), other.end());
}
private:
@ -117,59 +131,4 @@ private:
}
};
// This is the default Testable interface for *non*Testable elements, which
// uses stream operators.
template<typename VALUE, class ENABLE>
struct FastSetTestableHelper {
typedef FastSet<VALUE> Set;
static void print(const Set& set, const std::string& str) {
std::cout << str << "\n";
for (typename Set::const_iterator it = set.begin(); it != set.end(); ++it)
std::cout << " " << *it << "\n";
std::cout.flush();
}
static bool equals(const Set& set1, const Set& set2, double tol) {
typename Set::const_iterator it1 = set1.begin();
typename Set::const_iterator it2 = set2.begin();
while (it1 != set1.end()) {
if (it2 == set2.end() ||
fabs((double)(*it1) - (double)(*it2)) > tol)
return false;
++it1;
++it2;
}
return true;
}
};
// This is the Testable interface for Testable elements
template<typename VALUE>
struct FastSetTestableHelper<VALUE, typename boost::enable_if<has_print<VALUE> >::type> {
typedef FastSet<VALUE> Set;
static void print(const Set& set, const std::string& str) {
std::cout << str << "\n";
for (typename Set::const_iterator it = set.begin(); it != set.end(); ++it)
it->print(" ");
std::cout.flush();
}
static bool equals(const Set& set1, const Set& set2, double tol) {
typename Set::const_iterator it1 = set1.begin();
typename Set::const_iterator it2 = set2.begin();
while (it1 != set1.end()) {
if (it2 == set2.end() ||
!it1->equals(*it2, tol))
return false;
++it1;
++it2;
}
return true;
}
};
}

47
gtsam/base/FastVector.h
View File

@ -18,12 +18,9 @@
#pragma once
#include <vector>
#include <gtsam/base/FastDefaultAllocator.h>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/vector.hpp>
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastSet.h>
#include <vector>
namespace gtsam {
@ -35,20 +32,27 @@ namespace gtsam {
* @addtogroup base
*/
template<typename VALUE>
class FastVector: public std::vector<VALUE, typename internal::FastDefaultVectorAllocator<VALUE>::type> {
class FastVector: public std::vector<VALUE,
typename internal::FastDefaultVectorAllocator<VALUE>::type> {
public:
typedef std::vector<VALUE, typename internal::FastDefaultVectorAllocator<VALUE>::type> Base;
typedef std::vector<VALUE,
typename internal::FastDefaultVectorAllocator<VALUE>::type> Base;
/** Default constructor */
FastVector() {}
FastVector() {
}
/** Constructor from size */
explicit FastVector(size_t size) : Base(size) {}
explicit FastVector(size_t size) :
Base(size) {
}
/** Constructor from size and initial values */
explicit FastVector(size_t size, const VALUE& initial) : Base(size, initial) {}
explicit FastVector(size_t size, const VALUE& initial) :
Base(size, initial) {
}
/** Constructor from a range, passes through to base class */
template<typename INPUTITERATOR>
@ -56,33 +60,22 @@ public:
// This if statement works around a bug in boost pool allocator and/or
// STL vector where if the size is zero, the pool allocator will allocate
// huge amounts of memory.
if(first != last)
if (first != last)
Base::assign(first, last);
}
/** Copy constructor from a FastList */
FastVector(const FastList<VALUE>& x) {
if(x.size() > 0)
Base::assign(x.begin(), x.end());
}
/** Copy constructor from a FastSet */
FastVector(const FastSet<VALUE>& x) {
if(x.size() > 0)
Base::assign(x.begin(), x.end());
}
/** Copy constructor from the base class */
FastVector(const Base& x) : Base(x) {}
FastVector(const Base& x) :
Base(x) {
}
/** Copy constructor from a standard STL container */
template<typename ALLOCATOR>
FastVector(const std::vector<VALUE, ALLOCATOR>& x)
{
FastVector(const std::vector<VALUE, ALLOCATOR>& x) {
// This if statement works around a bug in boost pool allocator and/or
// STL vector where if the size is zero, the pool allocator will allocate
// huge amounts of memory.
if(x.size() > 0)
if (x.size() > 0)
Base::assign(x.begin(), x.end());
}

2
gtsam/base/Matrix.h
View File

@ -22,6 +22,8 @@
#pragma once
#include <gtsam/base/Vector.h>
#include <gtsam/config.h> // Configuration from CMake
#include <boost/math/special_functions/fpclassify.hpp>
#include <Eigen/Core>
#include <Eigen/Cholesky>

2
gtsam/base/OptionalJacobian.h
View File

@ -18,7 +18,7 @@
*/
#pragma once
#include <gtsam/config.h> // Configuration from CMake
#include <Eigen/Dense>
#ifndef OPTIONALJACOBIAN_NOBOOST

1
gtsam/base/SymmetricBlockMatrix.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/ThreadsafeException.h>
namespace gtsam {

23
gtsam/base/SymmetricBlockMatrix.h
View File

@ -17,9 +17,21 @@
*/
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/SymmetricBlockMatrixBlockExpr.h>
#include <gtsam/base/types.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <cassert>
#include <stdexcept>
#include <vector>
namespace boost {
namespace serialization {
class access;
} /* namespace serialization */
} /* namespace boost */
namespace gtsam {
@ -247,13 +259,8 @@ namespace gtsam {
}
};
/* ************************************************************************* */
class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
{
public:
CholeskyFailed() throw() {}
virtual ~CholeskyFailed() throw() {}
};
/// Foward declare exception class
class CholeskyFailed;
}

11
gtsam/base/TestableAssertions.h
View File

@ -17,13 +17,14 @@
#pragma once
#include <vector>
#include <map>
#include <iostream>
#include <gtsam/base/Testable.h>
#include <gtsam/global_includes.h>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <gtsam/global_includes.h>
#include <gtsam/base/Testable.h>
#include <map>
#include <iostream>
#include <vector>
namespace gtsam {

155
gtsam/base/ThreadsafeException.h Normal file
View File

@ -0,0 +1,155 @@
/* ----------------------------------------------------------------------------
* 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 ThreadSafeException.h
* @brief Base exception type that uses tbb_exception if GTSAM is compiled with TBB
* @author Richard Roberts
* @date Aug 21, 2010
* @addtogroup base
*/
#pragma once
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <boost/optional/optional.hpp>
#include <tbb/scalable_allocator.h>
#include <string>
#include <typeinfo>
#ifdef GTSAM_USE_TBB
#include <tbb/tbb_allocator.h>
#include <tbb/tbb_exception.h>
#include <iostream>
#endif
namespace gtsam {
/// Base exception type that uses tbb_exception if GTSAM is compiled with TBB.
template<class DERIVED>
class ThreadsafeException:
#ifdef GTSAM_USE_TBB
public tbb::tbb_exception
#else
public std::exception
#endif
{
#ifdef GTSAM_USE_TBB
private:
typedef tbb::tbb_exception Base;
protected:
typedef std::basic_string<char, std::char_traits<char>,
tbb::tbb_allocator<char> > String;
#else
private:
typedef std::exception Base;
protected:
typedef std::string String;
#endif
protected:
bool dynamic_; ///< Whether this object was moved
mutable boost::optional<String> description_; ///< Optional description
/// Default constructor is protected - may only be created from derived classes
ThreadsafeException() :
dynamic_(false) {
}
/// Copy constructor is protected - may only be created from derived classes
ThreadsafeException(const ThreadsafeException& other) :
Base(other), dynamic_(false) {
}
/// Construct with description string
ThreadsafeException(const std::string& description) :
dynamic_(false), description_(
String(description.begin(), description.end())) {
}
/// Default destructor doesn't have the throw()
virtual ~ThreadsafeException() throw () {
}
public:
// Implement functions for tbb_exception
#ifdef GTSAM_USE_TBB
virtual tbb::tbb_exception* move() throw () {
void* cloneMemory = scalable_malloc(sizeof(DERIVED));
if (!cloneMemory) {
std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl;
exit(-1);
}
DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast<DERIVED&>(*this));
clone->dynamic_ = true;
return clone;
}
virtual void destroy() throw () {
if (dynamic_) {
DERIVED* derivedPtr = static_cast<DERIVED*>(this);
derivedPtr->~DERIVED();
scalable_free(derivedPtr);
}
}
virtual void throw_self() {
throw *static_cast<DERIVED*>(this);
}
virtual const char* name() const throw () {
return typeid(DERIVED).name();
}
#endif
virtual const char* what() const throw () {
return description_ ? description_->c_str() : "";
}
};
/// Thread-safe runtime error exception
class RuntimeErrorThreadsafe: public ThreadsafeException<RuntimeErrorThreadsafe> {
public:
/// Construct with a string describing the exception
RuntimeErrorThreadsafe(const std::string& description) :
ThreadsafeException<RuntimeErrorThreadsafe>(description) {
}
};
/// Thread-safe out of range exception
class OutOfRangeThreadsafe: public ThreadsafeException<OutOfRangeThreadsafe> {
public:
/// Construct with a string describing the exception
OutOfRangeThreadsafe(const std::string& description) :
ThreadsafeException<OutOfRangeThreadsafe>(description) {
}
};
/// Thread-safe invalid argument exception
class InvalidArgumentThreadsafe: public ThreadsafeException<
InvalidArgumentThreadsafe> {
public:
/// Construct with a string describing the exception
InvalidArgumentThreadsafe(const std::string& description) :
ThreadsafeException<InvalidArgumentThreadsafe>(description) {
}
};
/// Indicate Cholesky factorization failure
class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
{
public:
CholeskyFailed() throw() {}
virtual ~CholeskyFailed() throw() {}
};
} // namespace gtsam

2
gtsam/base/debug.cpp
View File

@ -17,6 +17,8 @@
*/
#include <gtsam/base/debug.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#ifdef GTSAM_USE_TBB
#include <tbb/mutex.h>
#endif

9
gtsam/base/tests/testFastContainers.cpp
View File

@ -7,13 +7,14 @@
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/FastVector.h>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/set.hpp>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/FastVector.h>
#include <CppUnitLite/TestHarness.h>
using namespace boost::assign;
using namespace gtsam;
@ -21,7 +22,7 @@ using namespace gtsam;
/* ************************************************************************* */
TEST( testFastContainers, KeySet ) {
FastVector<size_t> init_vector;
FastVector<Key> init_vector;
init_vector += 2, 3, 4, 5;
FastSet<size_t> actSet(init_vector);

10
gtsam/base/tests/testSerializationBase.cpp
View File

@ -16,6 +16,8 @@
* @date Feb 7, 2012
*/
#include <gtsam/inference/Key.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/FastList.h>
@ -60,10 +62,10 @@ TEST (Serialization, FastMap) {
/* ************************************************************************* */
TEST (Serialization, FastSet) {
FastSet<double> set;
set.insert(1.0);
set.insert(2.0);
set.insert(3.0);
KeySet set;
set.insert(1);
set.insert(2);
set.insert(3);
EXPECT(equality(set));
EXPECT(equalityXML(set));

1
gtsam/base/timing.h
View File

@ -19,6 +19,7 @@
#include <gtsam/base/FastMap.h>
#include <gtsam/dllexport.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <boost/smart_ptr/shared_ptr.hpp>
#include <boost/smart_ptr/weak_ptr.hpp>

1
gtsam/base/treeTraversal-inst.h
View File

@ -22,6 +22,7 @@
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Key.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <stack>
#include <vector>

1
gtsam/base/treeTraversal/parallelTraversalTasks.h
View File

@ -24,6 +24,7 @@
#ifdef GTSAM_USE_TBB
# include <tbb/tbb.h>
# include <tbb/scalable_allocator.h>
# undef max // TBB seems to include windows.h and we don't want these macros
# undef min
# undef ERROR

35
gtsam/base/types.cpp
View File

@ -1,35 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file types.h
* @brief Typedefs for easier changing of types
* @author Richard Roberts
* @date Aug 21, 2010
* @addtogroup base
*/
#include <gtsam/base/types.h>
#include <gtsam/inference/Symbol.h>
#include <boost/lexical_cast.hpp>
namespace gtsam {
/* ************************************************************************* */
std::string _defaultKeyFormatter(Key key) {
const Symbol asSymbol(key);
if(asSymbol.chr() > 0)
return (std::string)asSymbol;
else
return boost::lexical_cast<std::string>(key);
}
}

120
gtsam/base/types.h
View File

@ -20,19 +20,16 @@
#pragma once
#include <gtsam/dllexport.h>
#include <gtsam/config.h>
#include <boost/function/function1.hpp>
#include <boost/concept/assert.hpp>
#include <boost/range/concepts.hpp>
#include <boost/optional.hpp>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <cstddef>
#include <string>
#include <ostream>
#ifdef GTSAM_USE_TBB
#include <tbb/task_scheduler_init.h>
#include <tbb/tbb_exception.h>
#include <tbb/scalable_allocator.h>
#include <iostream>
#endif
#ifdef GTSAM_USE_EIGEN_MKL_OPENMP
@ -58,22 +55,9 @@ namespace gtsam {
/// Integer nonlinear key type
typedef size_t Key;
/// Typedef for a function to format a key, i.e. to convert it to a string
typedef boost::function<std::string(Key)> KeyFormatter;
// Helper function for DefaultKeyFormatter
GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
/// The default KeyFormatter, which is used if no KeyFormatter is passed to
/// a nonlinear 'print' function. Automatically detects plain integer keys
/// and Symbol keys.
static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
/// The index type for Eigen objects
typedef ptrdiff_t DenseIndex;
/* ************************************************************************* */
/**
* Helper class that uses templates to select between two types based on
@ -148,104 +132,6 @@ namespace gtsam {
return ListOfOneContainer<T>(element);
}
/* ************************************************************************* */
/// Base exception type that uses tbb_exception if GTSAM is compiled with TBB.
template<class DERIVED>
class ThreadsafeException :
#ifdef GTSAM_USE_TBB
public tbb::tbb_exception
#else
public std::exception
#endif
{
#ifdef GTSAM_USE_TBB
private:
typedef tbb::tbb_exception Base;
protected:
typedef std::basic_string<char, std::char_traits<char>, tbb::tbb_allocator<char> > String;
#else
private:
typedef std::exception Base;
protected:
typedef std::string String;
#endif
protected:
bool dynamic_; ///< Whether this object was moved
mutable boost::optional<String> description_; ///< Optional description
/// Default constructor is protected - may only be created from derived classes
ThreadsafeException() : dynamic_(false) {}
/// Copy constructor is protected - may only be created from derived classes
ThreadsafeException(const ThreadsafeException& other) : Base(other), dynamic_(false) {}
/// Construct with description string
ThreadsafeException(const std::string& description) : dynamic_(false), description_(String(description.begin(), description.end())) {}
/// Default destructor doesn't have the throw()
virtual ~ThreadsafeException() throw () {}
public:
// Implement functions for tbb_exception
#ifdef GTSAM_USE_TBB
virtual tbb::tbb_exception* move() throw () {
void* cloneMemory = scalable_malloc(sizeof(DERIVED));
if (!cloneMemory) {
std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl;
exit(-1);
}
DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast<DERIVED&>(*this));
clone->dynamic_ = true;
return clone;
}
virtual void destroy() throw() {
if (dynamic_) {
DERIVED* derivedPtr = static_cast<DERIVED*>(this);
derivedPtr->~DERIVED();
scalable_free(derivedPtr);
}
}
virtual void throw_self() {
throw *static_cast<DERIVED*>(this); }
virtual const char* name() const throw() {
return typeid(DERIVED).name(); }
#endif
virtual const char* what() const throw() {
return description_ ? description_->c_str() : ""; }
};
/* ************************************************************************* */
/// Threadsafe runtime error exception
class RuntimeErrorThreadsafe : public ThreadsafeException<RuntimeErrorThreadsafe>
{
public:
/// Construct with a string describing the exception
RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException<RuntimeErrorThreadsafe>(description) {}
};
/* ************************************************************************* */
/// Threadsafe runtime error exception
class OutOfRangeThreadsafe : public ThreadsafeException<OutOfRangeThreadsafe>
{
public:
/// Construct with a string describing the exception
OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException<OutOfRangeThreadsafe>(description) {}
};
/* ************************************************************************* */
/// Threadsafe invalid argument exception
class InvalidArgumentThreadsafe : public ThreadsafeException<InvalidArgumentThreadsafe>
{
public:
/// Construct with a string describing the exception
InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException<InvalidArgumentThreadsafe>(description) {}
};
/* ************************************************************************* */
#ifdef __clang__
# pragma clang diagnostic push

4
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -39,8 +39,8 @@ namespace gtsam {
}
/* ************************************************************************* */
FastSet<Key> DiscreteFactorGraph::keys() const {
FastSet<Key> keys;
KeySet DiscreteFactorGraph::keys() const {
KeySet keys;
BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor) keys.insert(factor->begin(), factor->end());
return keys;

2
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -120,7 +120,7 @@ public:
}
/** Return the set of variables involved in the factors (set union) */
FastSet<Key> keys() const;
KeySet keys() const;
/** return product of all factors as a single factor */
DecisionTreeFactor product() const;

1
gtsam/discrete/Potentials.h
View File

@ -19,6 +19,7 @@
#include <gtsam/discrete/AlgebraicDecisionTree.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/inference/Key.h>
#include <boost/shared_ptr.hpp>
#include <set>

5
gtsam/geometry/CalibratedCamera.h
View File

@ -19,6 +19,11 @@
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/concepts.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/ThreadsafeException.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
namespace gtsam {

1
gtsam/geometry/Unit3.cpp
View File

@ -21,6 +21,7 @@
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <boost/random/mersenne_twister.hpp>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#ifdef __clang__
# pragma clang diagnostic push

4
gtsam/inference/BayesTree-inst.h
View File

@ -344,7 +344,7 @@ namespace gtsam {
gttic(Full_root_factoring);
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; {
FastVector<Key> C1_minus_B; {
FastSet<Key> C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
BOOST_FOREACH(const Key j, *B->conditional()) {
C1_minus_B_set.erase(j); }
C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
@ -356,7 +356,7 @@ namespace gtsam {
}
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
FastVector<Key> C2_minus_B; {
FastSet<Key> C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
BOOST_FOREACH(const Key j, *B->conditional()) {
C2_minus_B_set.erase(j); }
C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());

6
gtsam/inference/BayesTree.h
View File

@ -19,13 +19,13 @@
#pragma once
#include <string>
#include <gtsam/base/types.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastList.h>
#include <gtsam/base/ConcurrentMap.h>
#include <gtsam/base/FastVector.h>
#include <string>
namespace gtsam {
// Forward declarations

8
gtsam/inference/BayesTreeCliqueBase-inst.h
View File

@ -44,8 +44,8 @@ namespace gtsam {
FastVector<Key>
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
{
FastSet<Key> p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
FastVector<Key> S_setminus_B;
std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
@ -58,8 +58,8 @@ namespace gtsam {
const derived_ptr& B, const FactorGraphType& p_Cp_B) const
{
gttic(shortcut_indices);
FastSet<Key> allKeys = p_Cp_B.keys();
FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
KeySet allKeys = p_Cp_B.keys();
KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
FastVector<Key> S_setminus_B = separator_setminus_B(B);
FastVector<Key> keep;
// keep = S\B intersect allKeys (S_setminus_B is already sorted)

5
gtsam/inference/FactorGraph-inst.h
View File

@ -28,6 +28,7 @@
#include <stdio.h>
#include <sstream>
#include <iostream> // for cout :-(
namespace gtsam {
@ -72,8 +73,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
FastSet<Key> FactorGraph<FACTOR>::keys() const {
FastSet<Key> allKeys;
KeySet FactorGraph<FACTOR>::keys() const {
KeySet allKeys;
BOOST_FOREACH(const sharedFactor& factor, factors_)
if (factor)
allKeys.insert(factor->begin(), factor->end());

2
gtsam/inference/FactorGraph.h
View File

@ -332,7 +332,7 @@ namespace gtsam {
size_t nrFactors() const;
/** Potentially very slow function to return all keys involved */
FastSet<Key> keys() const;
KeySet keys() const;
/** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */
inline bool exists(size_t idx) const { return idx < size() && at(idx); }

4
gtsam/inference/ISAM-inst.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
// Remove the contaminated part of the Bayes tree
BayesNetType bn;
if (!this->empty()) {
const FastSet<Key> newFactorKeys = newFactors.keys();
const KeySet newFactorKeys = newFactors.keys();
this->removeTop(std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans);
}
@ -44,7 +44,7 @@ namespace gtsam {
// eliminate into a Bayes net
const VariableIndex varIndex(factors);
const FastSet<Key> newFactorKeys = newFactors.keys();
const KeySet newFactorKeys = newFactors.keys();
const Ordering constrainedOrdering =
Ordering::colamdConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex);

55
gtsam/inference/Key.cpp
View File

@ -17,57 +17,72 @@
* @date Feb 20, 2012
*/
#include <iostream>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/LabeledSymbol.h>
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <iostream>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
std::string _multirobotKeyFormatter(Key key) {
string _defaultKeyFormatter(Key key) {
const Symbol asSymbol(key);
if (asSymbol.chr() > 0)
return (string) asSymbol;
else
return boost::lexical_cast<string>(key);
}
/* ************************************************************************* */
void PrintKey(Key key, const string& s, const KeyFormatter& keyFormatter) {
cout << s << keyFormatter(key);
}
/* ************************************************************************* */
string _multirobotKeyFormatter(Key key) {
const LabeledSymbol asLabeledSymbol(key);
if (asLabeledSymbol.chr() > 0 && asLabeledSymbol.label() > 0)
return (std::string) asLabeledSymbol;
return (string) asLabeledSymbol;
const Symbol asSymbol(key);
if (asLabeledSymbol.chr() > 0)
return (std::string) asSymbol;
return (string) asSymbol;
else
return boost::lexical_cast<std::string>(key);
return boost::lexical_cast<string>(key);
}
/* ************************************************************************* */
template<class CONTAINER>
static void print(const CONTAINER& keys, const std::string& s,
static void Print(const CONTAINER& keys, const string& s,
const KeyFormatter& keyFormatter) {
std::cout << s << " ";
cout << s << " ";
if (keys.empty())
std::cout << "(none)" << std::endl;
cout << "(none)" << endl;
else {
BOOST_FOREACH(const Key& key, keys)
std::cout << keyFormatter(key) << " ";
std::cout << std::endl;
cout << keyFormatter(key) << " ";
cout << endl;
}
}
/* ************************************************************************* */
void printKeyList(const KeyList& keys, const std::string& s,
void PrintKeyList(const KeyList& keys, const string& s,
const KeyFormatter& keyFormatter) {
print(keys, s, keyFormatter);
Print(keys, s, keyFormatter);
}
/* ************************************************************************* */
void printKeyVector(const KeyVector& keys, const std::string& s,
void PrintKeyVector(const KeyVector& keys, const string& s,
const KeyFormatter& keyFormatter) {
print(keys, s, keyFormatter);
Print(keys, s, keyFormatter);
}
/* ************************************************************************* */
void printKeySet(const KeySet& keys, const std::string& s,
void PrintKeySet(const KeySet& keys, const string& s,
const KeyFormatter& keyFormatter) {
print(keys, s, keyFormatter);
Print(keys, s, keyFormatter);
}
/* ************************************************************************* */

90
gtsam/inference/Key.h
View File

@ -17,45 +17,75 @@
*/
#pragma once
#include <boost/function.hpp>
#include <string>
#include <gtsam/global_includes.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/types.h>
#include <gtsam/dllexport.h>
#include <boost/function.hpp>
namespace gtsam {
/// Typedef for a function to format a key, i.e. to convert it to a string
typedef boost::function<std::string(Key)> KeyFormatter;
// Helper function for Multi-robot Key Formatter
GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
// Helper function for DefaultKeyFormatter
GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
///
/// A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain
/// integer keys. This keyformatter will need to be passed in to override the default
/// formatter in print functions.
///
/// Checks for LabeledSymbol, Symbol and then plain keys, in order.
static const gtsam::KeyFormatter MultiRobotKeyFormatter = &_multirobotKeyFormatter;
/// The default KeyFormatter, which is used if no KeyFormatter is passed to
/// a nonlinear 'print' function. Automatically detects plain integer keys
/// and Symbol keys.
static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
/// Useful typedefs for operations with Values - allow for matlab interfaces
typedef FastList<Key> KeyList;
typedef FastVector<Key> KeyVector;
typedef FastSet<Key> KeySet;
typedef FastMap<Key,int> KeyGroupMap;
// Helper function for Multi-robot Key Formatter
GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
/// Utility function to print sets of keys with optional prefix
GTSAM_EXPORT void printKeyList(const KeyList& keys, const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
///
/// A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain
/// integer keys. This keyformatter will need to be passed in to override the default
/// formatter in print functions.
///
/// Checks for LabeledSymbol, Symbol and then plain keys, in order.
static const gtsam::KeyFormatter MultiRobotKeyFormatter =
&_multirobotKeyFormatter;
/// Utility function to print sets of keys with optional prefix
GTSAM_EXPORT void printKeyVector(const KeyVector& keys, const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/// Useful typedef for operations with Values - allows for matlab interface
typedef FastVector<Key> KeyVector;
/// Utility function to print sets of keys with optional prefix
GTSAM_EXPORT void printKeySet(const KeySet& keys, const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
}
// TODO(frank): Nothing fast about these :-(
typedef FastList<Key> KeyList;
typedef FastSet<Key> KeySet;
typedef FastMap<Key, int> KeyGroupMap;
/// Utility function to print one key with optional prefix
GTSAM_EXPORT void PrintKey(Key key, const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/// Utility function to print sets of keys with optional prefix
GTSAM_EXPORT void PrintKeyList(const KeyList& keys, const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/// Utility function to print sets of keys with optional prefix
GTSAM_EXPORT void PrintKeyVector(const KeyVector& keys, const std::string& s =
"", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/// Utility function to print sets of keys with optional prefix
GTSAM_EXPORT void PrintKeySet(const KeySet& keys, const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
// Define Key to be Testable by specializing gtsam::traits
template<typename T> struct traits;
template<> struct traits<Key> {
static void Print(const Key& key, const std::string& str = "") {
PrintKey(key, str);
}
static bool Equals(const Key& key1, const Key& key2, double tol = 1e-8) {
return key1 == key2;
}
};
} // namespace gtsam

4
gtsam/inference/MetisIndex-inl.h
View File

@ -26,8 +26,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
void MetisIndex::augment(const FactorGraph<FACTOR>& factors) {
std::map<int32_t, FastSet<int32_t> > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first
std::map<int32_t, FastSet<int32_t> >::iterator iAdjMapIt;
std::map<int32_t, std::set<int32_t> > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first
std::map<int32_t, std::set<int32_t> >::iterator iAdjMapIt;
std::set<Key> keySet;
/* ********** Convert to CSR format ********** */

2
gtsam/inference/Ordering.h
View File

@ -149,7 +149,7 @@ namespace gtsam {
/// Return a natural Ordering. Typically used by iterative solvers
template <class FACTOR>
static Ordering Natural(const FactorGraph<FACTOR> &fg) {
FastSet<Key> src = fg.keys();
KeySet src = fg.keys();
std::vector<Key> keys(src.begin(), src.end());
std::stable_sort(keys.begin(), keys.end());
return Ordering(keys);

2
gtsam/inference/VariableIndex-inl.h
View File

@ -18,6 +18,8 @@
#pragma once
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
namespace gtsam {

12
gtsam/inference/VariableIndex.h
View File

@ -18,16 +18,14 @@
#pragma once
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/types.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/dllexport.h>
#include <boost/foreach.hpp>
#include <boost/optional/optional.hpp>
#include <boost/smart_ptr/shared_ptr.hpp>
#include <vector>
#include <deque>
#include <cassert>
#include <stdexcept>
namespace gtsam {

1
gtsam/inference/tests/testKey.cpp
View File

@ -14,6 +14,7 @@
* @author Alex Cunningham
*/
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>

2
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -47,7 +47,7 @@ namespace gtsam {
/* ************************************************************************* */
GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
FastSet<Key> keys;
KeySet keys;
BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor)
keys.insert(factor->begin(), factor->end());

2
gtsam/linear/GaussianFactorGraph.h
View File

@ -135,7 +135,7 @@ namespace gtsam {
* Return the set of variables involved in the factors (computes a set
* union).
*/
typedef FastSet<Key> Keys;
typedef KeySet Keys;
Keys keys() const;
/* return a map of (Key, dimension) */

18
gtsam/linear/HessianFactor.cpp
View File

@ -15,17 +15,19 @@
* @date Dec 8, 2010
*/
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/ThreadsafeException.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
@ -469,7 +471,7 @@ std::pair<boost::shared_ptr<GaussianConditional>,
// Erase the eliminated keys in the remaining factor
jointFactor->keys_.erase(jointFactor->begin(),
jointFactor->begin() + numberOfKeysToEliminate);
} catch (CholeskyFailed&) {
} catch (const CholeskyFailed& e) {
throw IndeterminantLinearSystemException(keys.front());
}

97
gtsam/linear/RegularHessianFactor.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/RegularJacobianFactor.h>
#include <boost/foreach.hpp>
#include <vector>
@ -27,15 +28,11 @@ namespace gtsam {
template<size_t D>
class RegularHessianFactor: public HessianFactor {
private:
// Use Eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
public:
typedef Eigen::Matrix<double, D, 1> VectorD;
typedef Eigen::Matrix<double, D, D> MatrixD;
/** Construct an n-way factor. Gs contains the upper-triangle blocks of the
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces
* of the linear vector term, and f the constant term.
@ -43,27 +40,68 @@ public:
RegularHessianFactor(const std::vector<Key>& js,
const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
HessianFactor(js, Gs, gs, f) {
checkInvariants();
}
/** Construct a binary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term.
*/
RegularHessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12,
const Vector& g1, const Matrix& G22, const Vector& g2, double f) :
RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12,
const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) :
HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) {
}
/** Construct a ternary factor. Gxx are the upper-triangle blocks of the
* quadratic term (the Hessian matrix), gx the pieces of the linear vector
* term, and f the constant term.
*/
RegularHessianFactor(Key j1, Key j2, Key j3,
const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1,
const MatrixD& G22, const MatrixD& G23, const VectorD& g2,
const MatrixD& G33, const VectorD& g3, double f) :
HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) {
}
/** Constructor with an arbitrary number of keys and with the augmented information matrix
* specified as a block matrix. */
template<typename KEYS>
RegularHessianFactor(const KEYS& keys,
const SymmetricBlockMatrix& augmentedInformation) :
HessianFactor(keys, augmentedInformation) {
checkInvariants();
}
/// Construct from RegularJacobianFactor
RegularHessianFactor(const RegularJacobianFactor<D>& jf)
: HessianFactor(jf) {}
/// Construct from a GaussianFactorGraph
RegularHessianFactor(const GaussianFactorGraph& factors,
boost::optional<const Scatter&> scatter = boost::none)
: HessianFactor(factors, scatter) {
checkInvariants();
}
private:
/// Check invariants after construction
void checkInvariants() {
if (info_.cols() != 1 + (info_.nBlocks()-1) * (DenseIndex)D)
throw std::invalid_argument(
"RegularHessianFactor constructor was given non-regular factors");
}
// Use Eigen magic to access raw memory
typedef Eigen::Map<VectorD> DMap;
typedef Eigen::Map<const VectorD> ConstDMap;
// Scratch space for multiplyHessianAdd
mutable std::vector<DVector> y;
// According to link below this is thread-safe.
// http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe
mutable std::vector<VectorD> y_;
public:
/** y += alpha * A'*A*x */
virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
@ -74,32 +112,32 @@ public:
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const double* x,
double* yvalues) const {
// Create a vector of temporary y values, corresponding to rows i
y.resize(size());
BOOST_FOREACH(DVector & yi, y)
// Create a vector of temporary y_ values, corresponding to rows i
y_.resize(size());
BOOST_FOREACH(VectorD & yi, y_)
yi.setZero();
// Accessing the VectorValues one by one is expensive
// So we will loop over columns to access x only once per column
// And fill the above temporary y values, to be added into yvalues after
DVector xj(D);
// And fill the above temporary y_ values, to be added into yvalues after
VectorD xj(D);
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
Key key = keys_[j];
const double* xj = x + key * D;
DenseIndex i = 0;
for (; i < j; ++i)
y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj);
y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj);
// blocks on the diagonal are only half
y[i] += info_(j, j).selfadjointView() * ConstDMap(xj);
y_[i] += info_(j, j).selfadjointView() * ConstDMap(xj);
// for below diagonal, we take transpose block from upper triangular part
for (i = j + 1; i < (DenseIndex) size(); ++i)
y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj);
y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj);
}
// copy to yvalues
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
Key key = keys_[i];
DMap(yvalues + key * D) += alpha * y[i];
DMap(yvalues + key * D) += alpha * y_[i];
}
}
@ -107,28 +145,27 @@ public:
void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
std::vector<size_t> offsets) const {
// Create a vector of temporary y values, corresponding to rows i
std::vector<Vector> y;
y.reserve(size());
for (const_iterator it = begin(); it != end(); it++)
y.push_back(zero(getDim(it)));
// Create a vector of temporary y_ values, corresponding to rows i
y_.resize(size());
BOOST_FOREACH(VectorD & yi, y_)
yi.setZero();
// Accessing the VectorValues one by one is expensive
// So we will loop over columns to access x only once per column
// And fill the above temporary y values, to be added into yvalues after
// And fill the above temporary y_ values, to be added into yvalues after
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
DenseIndex i = 0;
for (; i < j; ++i)
y[i] += info_(i, j).knownOffDiagonal()
y_[i] += info_(i, j).knownOffDiagonal()
* ConstDMap(x + offsets[keys_[j]],
offsets[keys_[j] + 1] - offsets[keys_[j]]);
// blocks on the diagonal are only half
y[i] += info_(j, j).selfadjointView()
y_[i] += info_(j, j).selfadjointView()
* ConstDMap(x + offsets[keys_[j]],
offsets[keys_[j] + 1] - offsets[keys_[j]]);
// for below diagonal, we take transpose block from upper triangular part
for (i = j + 1; i < (DenseIndex) size(); ++i)
y[i] += info_(i, j).knownOffDiagonal()
y_[i] += info_(i, j).knownOffDiagonal()
* ConstDMap(x + offsets[keys_[j]],
offsets[keys_[j] + 1] - offsets[keys_[j]]);
}
@ -136,7 +173,7 @@ public:
// copy to yvalues
for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
DMap(yvalues + offsets[keys_[i]],
offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y[i];
offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y_[i];
}
/** Return the diagonal of the Hessian for this factor (raw memory version) */
@ -158,7 +195,7 @@ public:
for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
DVector dj = -info_(pos, size()).knownOffDiagonal();
VectorD dj = -info_(pos, size()).knownOffDiagonal();
DMap(d + D * j) += dj;
}
}

34
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -15,20 +15,40 @@
* @author: Frank Dellaert
*/
#include <gtsam/base/DSFVector.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/base/DSFVector.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/types.h>
#include <gtsam/base/Vector.h>
#include <boost/algorithm/string.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <numeric>
#include <iterator>
#include <list>
#include <map>
#include <numeric> // accumulate
#include <queue>
#include <set>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

20
gtsam/linear/SubgraphPreconditioner.h
View File

@ -17,13 +17,31 @@
#pragma once
#include <gtsam/global_includes.h>
#include <gtsam/linear/Errors.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/types.h>
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <boost/shared_ptr.hpp>
#include <map>
#include <utility>
#include <vector>
namespace boost {
namespace serialization {
class access;
} /* namespace serialization */
} /* namespace boost */
namespace gtsam {
// Forward declarations

1
gtsam/linear/linearExceptions.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/linear/linearExceptions.h>
#include <gtsam/inference/Symbol.h>
#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>
namespace gtsam {

5
gtsam/linear/linearExceptions.h
View File

@ -17,9 +17,8 @@
*/
#pragma once
#include <gtsam/global_includes.h>
#include <boost/lexical_cast.hpp>
#include <exception>
#include <gtsam/base/ThreadsafeException.h>
#include <gtsam/base/types.h>
namespace gtsam {

87
gtsam/linear/tests/testRegularHessianFactor.cpp
View File

@ -16,6 +16,7 @@
*/
#include <gtsam/linear/RegularHessianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <CppUnitLite/TestHarness.h>
@ -29,30 +30,62 @@ using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */
TEST(RegularHessianFactor, ConstructorNWay)
TEST(RegularHessianFactor, Constructors)
{
Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114).finished();
Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124).finished();
Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134).finished();
// First construct a regular JacobianFactor
// 0.5*|x0 + x1 + x3 - [1;2]|^2 = 0.5*|A*x-b|^2, with A=[I I I]
Matrix A1 = I_2x2, A2 = I_2x2, A3 = I_2x2;
Vector2 b(1,2);
vector<pair<Key, Matrix> > terms;
terms += make_pair(0, A1), make_pair(1, A2), make_pair(3, A3);
RegularJacobianFactor<2> jf(terms, b);
Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224).finished();
Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234).finished();
// Test conversion from JacobianFactor
RegularHessianFactor<2> factor(jf);
Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334).finished();
// 0.5*|A*x-b|^2 = 0.5*(Ax-b)'*(Ax-b) = 0.5*x'*A'A*x - x'*A'b + 0.5*b'*b
// Compare with comment in HessianFactor: E(x) = 0.5 x^T G x - x^T g + 0.5 f
// Hence G = I6, g A'*b = [b;b;b], and f = b'*b = 1+4 = 5
Matrix G11 = I_2x2;
Matrix G12 = I_2x2;
Matrix G13 = I_2x2;
Vector g1 = (Vector(2) << -7, -9).finished();
Vector g2 = (Vector(2) << -9, 1).finished();
Vector g3 = (Vector(2) << 2, 3).finished();
Matrix G22 = I_2x2;
Matrix G23 = I_2x2;
double f = 10;
Matrix G33 = I_2x2;
std::vector<Key> js;
js.push_back(0); js.push_back(1); js.push_back(3);
std::vector<Matrix> Gs;
Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
std::vector<Vector> gs;
gs.push_back(g1); gs.push_back(g2); gs.push_back(g3);
RegularHessianFactor<2> factor(js, Gs, gs, f);
Vector2 g1 = b, g2 = b, g3 = b;
double f = 5;
// Test ternary constructor
RegularHessianFactor<2> factor2(0, 1, 3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
EXPECT(assert_equal(factor,factor2));
// Test n-way constructor
vector<Key> keys; keys += 0, 1, 3;
vector<Matrix> Gs; Gs += G11, G12, G13, G22, G23, G33;
vector<Vector> gs; gs += g1, g2, g3;
RegularHessianFactor<2> factor3(keys, Gs, gs, f);
EXPECT(assert_equal(factor, factor3));
// Test constructor from Gaussian Factor Graph
GaussianFactorGraph gfg;
gfg += jf;
RegularHessianFactor<2> factor4(gfg);
EXPECT(assert_equal(factor, factor4));
GaussianFactorGraph gfg2;
gfg2 += factor;
RegularHessianFactor<2> factor5(gfg);
EXPECT(assert_equal(factor, factor5));
// Test constructor from Information matrix
Matrix info = factor.augmentedInformation();
vector<size_t> dims; dims += 2, 2, 2;
SymmetricBlockMatrix sym(dims, info, true);
RegularHessianFactor<2> factor6(keys, sym);
EXPECT(assert_equal(factor, factor6));
// multiplyHessianAdd:
{
@ -61,13 +94,13 @@ TEST(RegularHessianFactor, ConstructorNWay)
HessianFactor::const_iterator i1 = factor.begin();
HessianFactor::const_iterator i2 = i1 + 1;
Vector X(6); X << 1,2,3,4,5,6;
Vector Y(6); Y << 2633, 2674, 4465, 4501, 5669, 5696;
Vector Y(6); Y << 9, 12, 9, 12, 9, 12;
EXPECT(assert_equal(Y,AtA*X));
VectorValues x = map_list_of<Key, Vector>
(0, (Vector(2) << 1,2).finished())
(1, (Vector(2) << 3,4).finished())
(3, (Vector(2) << 5,6).finished());
(0, Vector2(1,2))
(1, Vector2(3,4))
(3, Vector2(5,6));
VectorValues expected;
expected.insert(0, Y.segment<2>(0));
@ -77,15 +110,15 @@ TEST(RegularHessianFactor, ConstructorNWay)
// VectorValues version
double alpha = 1.0;
VectorValues actualVV;
actualVV.insert(0, zero(2));
actualVV.insert(1, zero(2));
actualVV.insert(3, zero(2));
actualVV.insert(0, Vector2::Zero());
actualVV.insert(1, Vector2::Zero());
actualVV.insert(3, Vector2::Zero());
factor.multiplyHessianAdd(alpha, x, actualVV);
EXPECT(assert_equal(expected, actualVV));
// RAW ACCESS
Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696;
Vector fast_y = gtsam::zero(8);
Vector expected_y(8); expected_y << 9, 12, 9, 12, 0, 0, 9, 12;
Vector fast_y = Vector8::Zero();
double xvalues[8] = {1,2,3,4,0,0,5,6};
factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
EXPECT(assert_equal(expected_y, fast_y));

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

@ -19,6 +19,8 @@
#include <gtsam/nonlinear/ISAM2-impl.h>
#include <gtsam/inference/Symbol.h> // for selective linearization thresholds
#include <gtsam/base/debug.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <functional>
#include <boost/range/adaptors.hpp>
@ -84,11 +86,11 @@ void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool u
}
/* ************************************************************************* */
void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
Values& theta, VariableIndex& variableIndex,
VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd,
FastSet<Key>& replacedKeys, Base::Nodes& nodes,
FastSet<Key>& fixedVariables)
KeySet& replacedKeys, Base::Nodes& nodes,
KeySet& fixedVariables)
{
variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end());
BOOST_FOREACH(Key key, unusedKeys) {
@ -103,10 +105,10 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const FastVect
}
/* ************************************************************************* */
FastSet<Key> ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold)
{
FastSet<Key> relinKeys;
KeySet relinKeys;
if(const double* threshold = boost::get<double>(&relinearizeThreshold))
{
@ -131,7 +133,7 @@ FastSet<Key> ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
}
/* ************************************************************************* */
void CheckRelinearizationRecursiveDouble(FastSet<Key>& relinKeys, double threshold,
void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold,
const VectorValues& delta, const ISAM2Clique::shared_ptr& clique)
{
// Check the current clique for relinearization
@ -153,7 +155,7 @@ void CheckRelinearizationRecursiveDouble(FastSet<Key>& relinKeys, double thresho
}
/* ************************************************************************* */
void CheckRelinearizationRecursiveMap(FastSet<Key>& relinKeys, const FastMap<char,Vector>& thresholds,
void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap<char,Vector>& thresholds,
const VectorValues& delta,
const ISAM2Clique::shared_ptr& clique)
{
@ -185,11 +187,11 @@ void CheckRelinearizationRecursiveMap(FastSet<Key>& relinKeys, const FastMap<cha
}
/* ************************************************************************* */
FastSet<Key> ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold)
{
FastSet<Key> relinKeys;
KeySet relinKeys;
BOOST_FOREACH(const ISAM2::sharedClique& root, roots) {
if(relinearizeThreshold.type() == typeid(double))
CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root);
@ -200,7 +202,7 @@ FastSet<Key> ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sh
}
/* ************************************************************************* */
void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Key>& keys, const FastSet<Key>& markedMask)
void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask)
{
static const bool debug = false;
// does the separator contain any of the variables?
@ -224,7 +226,7 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Key>& keys, co
/* ************************************************************************* */
void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta,
const FastSet<Key>& mask, boost::optional<VectorValues&> invalidateIfDebug, const KeyFormatter& keyFormatter)
const KeySet& mask, boost::optional<VectorValues&> invalidateIfDebug, const KeyFormatter& keyFormatter)
{
// If debugging, invalidate if requested, otherwise do not invalidate.
// Invalidating means setting expmapped entries to Inf, to trigger assertions
@ -272,7 +274,7 @@ inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique,
/* ************************************************************************* */
size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
const FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold) {
const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold) {
size_t lastBacksubVariableCount;
@ -300,7 +302,7 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>
/* ************************************************************************* */
namespace internal {
void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const FastSet<Key>& replacedKeys,
void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const KeySet& replacedKeys,
const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) {
// Check if any frontal or separator keys were recalculated, if so, we need
@ -344,7 +346,7 @@ void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const FastSet<Ke
}
/* ************************************************************************* */
size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const FastSet<Key>& replacedKeys,
size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys,
const VectorValues& gradAtZero, VectorValues& RgProd) {
// Update variables

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

@ -57,10 +57,10 @@ struct GTSAM_EXPORT ISAM2::Impl {
/**
* Remove variables from the ISAM2 system.
*/
static void RemoveVariables(const FastSet<Key>& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
static void RemoveVariables(const KeySet& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton,
VectorValues& RgProd, FastSet<Key>& replacedKeys, Base::Nodes& nodes,
FastSet<Key>& fixedVariables);
VectorValues& RgProd, KeySet& replacedKeys, Base::Nodes& nodes,
KeySet& fixedVariables);
/**
* Find the set of variables to be relinearized according to relinearizeThreshold.
@ -71,7 +71,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold
*/
static FastSet<Key> CheckRelinearizationFull(const VectorValues& delta,
static KeySet CheckRelinearizationFull(const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
/**
@ -85,7 +85,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold
*/
static FastSet<Key> CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
static KeySet CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
/**
@ -103,7 +103,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
*
* Alternatively could we trace up towards the root for each variable here?
*/
static void FindAll(ISAM2Clique::shared_ptr clique, FastSet<Key>& keys, const FastSet<Key>& markedMask);
static void FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask);
/**
* Apply expmap to the given values, but only for indices appearing in
@ -119,7 +119,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
static void ExpmapMasked(Values& values, const VectorValues& delta,
const FastSet<Key>& mask,
const KeySet& mask,
boost::optional<VectorValues&> invalidateIfDebug = boost::none,
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
@ -127,13 +127,13 @@ struct GTSAM_EXPORT ISAM2::Impl {
* Update the Newton's method step point, using wildfire
*/
static size_t UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
const FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold);
const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold);
/**
* Update the RgProd (R*g) incrementally taking into account which variables
* have been recalculated in \c replacedKeys. Only used in Dogleg.
*/
static size_t UpdateRgProd(const ISAM2::Roots& roots, const FastSet<Key>& replacedKeys,
static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys,
const VectorValues& gradAtZero, VectorValues& RgProd);
/**

12
gtsam/nonlinear/ISAM2-inl.h
View File

@ -36,7 +36,7 @@ VALUE ISAM2::calculateEstimate(Key key) const {
namespace internal {
template<class CLIQUE>
void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
FastSet<Key>& changed, const FastSet<Key>& replaced, VectorValues& delta, size_t& count)
KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count)
{
// if none of the variables in this clique (frontal and separator!) changed
// significantly, then by the running intersection property, none of the
@ -113,7 +113,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
template<class CLIQUE>
bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double threshold,
FastSet<Key>& changed, const FastSet<Key>& replaced, VectorValues& delta, size_t& count)
KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count)
{
// if none of the variables in this clique (frontal and separator!) changed
// significantly, then by the running intersection property, none of the
@ -245,8 +245,8 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
/* ************************************************************************* */
template<class CLIQUE>
size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const FastSet<Key>& keys, VectorValues& delta) {
FastSet<Key> changed;
size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const KeySet& keys, VectorValues& delta) {
KeySet changed;
int count = 0;
// starting from the root, call optimize on each conditional
if(root)
@ -256,9 +256,9 @@ size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold,
/* ************************************************************************* */
template<class CLIQUE>
size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root, double threshold, const FastSet<Key>& keys, VectorValues& delta)
size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root, double threshold, const KeySet& keys, VectorValues& delta)
{
FastSet<Key> changed;
KeySet changed;
size_t count = 0;
if (root) {

38
gtsam/nonlinear/ISAM2.cpp
View File

@ -194,7 +194,7 @@ FastSet<size_t> ISAM2::getAffectedFactors(const FastList<Key>& keys) const {
// (note that the remaining stuff is summarized in the cached factors)
GaussianFactorGraph::shared_ptr
ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const FastSet<Key>& relinKeys) const
ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySet& relinKeys) const
{
gttic(getAffectedFactors);
FastSet<size_t> candidates = getAffectedFactors(affectedKeys);
@ -204,7 +204,7 @@ ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const FastS
gttic(affectedKeysSet);
// for fast lookup below
FastSet<Key> affectedKeysSet;
KeySet affectedKeysSet;
affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end());
gttoc(affectedKeysSet);
@ -260,9 +260,9 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
}
/* ************************************************************************* */
boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKeys, const FastSet<Key>& relinKeys,
boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const KeySet& relinKeys,
const vector<Key>& observedKeys,
const FastSet<Key>& unusedIndices,
const KeySet& unusedIndices,
const boost::optional<FastMap<Key,int> >& constrainKeys,
ISAM2Result& result)
{
@ -326,7 +326,7 @@ boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKe
affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals());
gttoc(affectedKeys);
boost::shared_ptr<FastSet<Key> > affectedKeysSet(new FastSet<Key>()); // Will return this result
boost::shared_ptr<KeySet > affectedKeysSet(new KeySet()); // Will return this result
if(affectedKeys.size() >= theta_.size() * batchThreshold)
{
@ -566,17 +566,17 @@ ISAM2Result ISAM2::update(
variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), removeFactors);
// Compute unused keys and indices
FastSet<Key> unusedKeys;
FastSet<Key> unusedIndices;
KeySet unusedKeys;
KeySet unusedIndices;
{
// Get keys from removed factors and new factors, and compute unused keys,
// i.e., keys that are empty now and do not appear in the new factors.
FastSet<Key> removedAndEmpty;
KeySet removedAndEmpty;
BOOST_FOREACH(Key key, removeFactors.keys()) {
if(variableIndex_[key].empty())
removedAndEmpty.insert(removedAndEmpty.end(), key);
}
FastSet<Key> newFactorSymbKeys = newFactors.keys();
KeySet newFactorSymbKeys = newFactors.keys();
std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end()));
@ -602,10 +602,10 @@ ISAM2Result ISAM2::update(
gttic(gather_involved_keys);
// 3. Mark linear update
FastSet<Key> markedKeys = newFactors.keys(); // Get keys from new factors
KeySet markedKeys = newFactors.keys(); // Get keys from new factors
// Also mark keys involved in removed factors
{
FastSet<Key> markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors
KeySet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors
markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys
}
// Also mark any provided extra re-eliminate keys
@ -632,7 +632,7 @@ ISAM2Result ISAM2::update(
gttoc(gather_involved_keys);
// Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
FastSet<Key> relinKeys;
KeySet relinKeys;
if (relinearizeThisStep) {
gttic(gather_relinearize_keys);
// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
@ -659,7 +659,7 @@ ISAM2Result ISAM2::update(
result.detail->variableStatus[key].isRelinearized = true; } }
// Add the variables being relinearized to the marked keys
FastSet<Key> markedRelinMask;
KeySet markedRelinMask;
BOOST_FOREACH(const Key key, relinKeys)
markedRelinMask.insert(key);
markedKeys.insert(relinKeys.begin(), relinKeys.end());
@ -674,7 +674,7 @@ ISAM2Result ISAM2::update(
// Relin involved keys for detailed results
if(params_.enableDetailedResults) {
FastSet<Key> involvedRelinKeys;
KeySet involvedRelinKeys;
BOOST_FOREACH(const sharedClique& root, roots_)
Impl::FindAll(root, involvedRelinKeys, markedRelinMask);
BOOST_FOREACH(Key key, involvedRelinKeys) {
@ -726,7 +726,7 @@ ISAM2Result ISAM2::update(
gttic(recalculate);
// 8. Redo top of Bayes tree
boost::shared_ptr<FastSet<Key> > replacedKeys;
boost::shared_ptr<KeySet > replacedKeys;
if(!markedKeys.empty() || !observedKeys.empty())
replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedKeys, result);
@ -758,7 +758,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
boost::optional<std::vector<size_t>&> deletedFactorsIndices)
{
// Convert to ordered set
FastSet<Key> leafKeys(leafKeysList.begin(), leafKeysList.end());
KeySet leafKeys(leafKeysList.begin(), leafKeysList.end());
// Keep track of marginal factors - map from clique to the marginal factors
// that should be incorporated into it, passed up from it's children.
@ -769,7 +769,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
FastSet<size_t> factorIndicesToRemove;
// Keep track of variables removed in subtrees
FastSet<Key> leafKeysRemoved;
KeySet leafKeysRemoved;
// Remove each variable and its subtrees
BOOST_FOREACH(Key j, leafKeys) {
@ -881,7 +881,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
graph.push_back(nonlinearFactors_[i]->linearize(theta_)); }
// Reeliminate the linear graph to get the marginal and discard the conditional
const FastSet<Key> cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals());
const KeySet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals());
FastVector<Key> cliqueFrontalsToEliminate;
std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(),
std::back_inserter(cliqueFrontalsToEliminate));
@ -957,7 +957,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
deletedFactorsIndices->assign(factorIndicesToRemove.begin(), factorIndicesToRemove.end());
// Remove the marginalized variables
Impl::RemoveVariables(FastSet<Key>(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
deltaReplacedMask_, nodes_, fixedVariables_);
}

16
gtsam/nonlinear/ISAM2.h
View File

@ -460,7 +460,7 @@ protected:
* This is \c mutable because it is used internally to not update delta_
* until it is needed.
*/
mutable FastSet<Key> deltaReplacedMask_; // TODO: Make sure accessed in the right way
mutable KeySet deltaReplacedMask_; // TODO: Make sure accessed in the right way
/** All original nonlinear factors are stored here to use during relinearization */
NonlinearFactorGraph nonlinearFactors_;
@ -476,7 +476,7 @@ protected:
/** Set of variables that are involved with linear factors from marginalized
* variables and thus cannot have their linearization points changed. */
FastSet<Key> fixedVariables_;
KeySet fixedVariables_;
int update_count_; ///< Counter incremented every update(), used to determine periodic relinearization
@ -614,7 +614,7 @@ public:
const VariableIndex& getVariableIndex() const { return variableIndex_; }
/** Access the nonlinear variable index */
const FastSet<Key>& getFixedVariables() const { return fixedVariables_; }
const KeySet& getFixedVariables() const { return fixedVariables_; }
size_t lastAffectedVariableCount;
size_t lastAffectedFactorCount;
@ -641,11 +641,11 @@ public:
protected:
FastSet<size_t> getAffectedFactors(const FastList<Key>& keys) const;
GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const FastSet<Key>& relinKeys) const;
GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySet& relinKeys) const;
GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
virtual boost::shared_ptr<FastSet<Key> > recalculate(const FastSet<Key>& markedKeys, const FastSet<Key>& relinKeys,
const std::vector<Key>& observedKeys, const FastSet<Key>& unusedIndices, const boost::optional<FastMap<Key,int> >& constrainKeys, ISAM2Result& result);
virtual boost::shared_ptr<KeySet > recalculate(const KeySet& markedKeys, const KeySet& relinKeys,
const std::vector<Key>& observedKeys, const KeySet& unusedIndices, const boost::optional<FastMap<Key,int> >& constrainKeys, ISAM2Result& result);
void updateDelta(bool forceFullSolve = false) const;
}; // ISAM2
@ -666,11 +666,11 @@ template<> struct traits<ISAM2> : public Testable<ISAM2> {};
/// @return The number of variables that were solved for
template<class CLIQUE>
size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
double threshold, const FastSet<Key>& replaced, VectorValues& delta);
double threshold, const KeySet& replaced, VectorValues& delta);
template<class CLIQUE>
size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root,
double threshold, const FastSet<Key>& replaced, VectorValues& delta);
double threshold, const KeySet& replaced, VectorValues& delta);
/// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
template<class CLIQUE>

22
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -17,22 +17,24 @@
* @author Christian Potthast
*/
#include <cmath>
#include <limits>
#include <boost/foreach.hpp>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#ifdef GTSAM_USE_TBB
# include <tbb/parallel_for.h>
#endif
#include <boost/foreach.hpp>
#include <cmath>
#include <limits>
using namespace std;
namespace gtsam {
@ -70,7 +72,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
stm << " size=\"" << formatting.figureWidthInches << "," <<
formatting.figureHeightInches << "\";\n\n";
FastSet<Key> keys = this->keys();
KeySet keys = this->keys();
// Local utility function to extract x and y coordinates
struct { boost::optional<Point2> operator()(
@ -144,7 +146,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
if (formatting.mergeSimilarFactors) {
// Remove duplicate factors
FastSet<vector<Key> > structure;
std::set<vector<Key> > structure;
BOOST_FOREACH(const sharedFactor& factor, *this){
if(factor) {
vector<Key> factorKeys = factor->keys();
@ -234,8 +236,8 @@ double NonlinearFactorGraph::error(const Values& c) const {
}
/* ************************************************************************* */
FastSet<Key> NonlinearFactorGraph::keys() const {
FastSet<Key> keys;
KeySet NonlinearFactorGraph::keys() const {
KeySet keys;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor)
keys.insert(factor->begin(), factor->end());

2
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -106,7 +106,7 @@ namespace gtsam {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** return keys as an ordered set - ordering is by key value */
FastSet<Key> keys() const;
KeySet keys() const;
/** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */
double error(const Values& c) const;

2
gtsam/nonlinear/internal/ExecutionTrace.h
View File

@ -17,7 +17,7 @@
*/
#pragma once
#include <gtsam/config.h> // Configuration from CMake
#include <gtsam/nonlinear/internal/JacobianMap.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/Manifold.h>

4
gtsam/symbolic/SymbolicFactor-inst.h
View File

@ -38,7 +38,7 @@ namespace gtsam
EliminateSymbolic(const FactorGraph<FACTOR>& factors, const Ordering& keys)
{
// Gather all keys
FastSet<Key> allKeys;
KeySet allKeys;
BOOST_FOREACH(const boost::shared_ptr<FACTOR>& factor, factors) {
allKeys.insert(factor->begin(), factor->end());
}
@ -50,7 +50,7 @@ namespace gtsam
}
// Sort frontal keys
FastSet<Key> frontals(keys);
KeySet frontals(keys);
const size_t nFrontals = keys.size();
// Build a key vector with the frontals followed by the separator

2
gtsam_unstable/discrete/tests/testLoopyBelief.cpp
View File

@ -193,7 +193,7 @@ private:
// add the belief factor for each neighbor variable to this star graph
// also record the factor index for later modification
FastSet<Key> neighbors = star->keys();
KeySet neighbors = star->keys();
neighbors.erase(key);
CorrectedBeliefIndices correctedBeliefIndices;
BOOST_FOREACH(Key neighbor, neighbors) {

2
gtsam_unstable/linear/QPSolver.h
View File

@ -48,7 +48,7 @@ class QPSolver {
GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. The working set of inequalities will be added to this base graph in the process.
VariableIndex costVariableIndex_, equalityVariableIndex_,
inequalityVariableIndex_;
FastSet<Key> constrainedKeys_; //!< all constrained keys, will become factors in the dual graph
KeySet constrainedKeys_; //!< all constrained keys, will become factors in the dual graph
public:
/// Constructor

6
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -463,7 +463,7 @@ void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
}
/* ************************************************************************* */
void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet<Key>& keys,
void BatchFixedLagSmoother::PrintKeySet(const gtsam::KeySet& keys,
const std::string& label) {
std::cout << label;
BOOST_FOREACH(gtsam::Key key, keys) {
@ -531,13 +531,13 @@ NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(
"BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: ");
// Get the set of all keys involved in the factor graph
FastSet<Key> allKeys(graph.keys());
KeySet allKeys(graph.keys());
if (debug)
PrintKeySet(allKeys,
"BatchFixedLagSmoother::calculateMarginalFactors All Keys: ");
// Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys
FastSet<Key> remainingKeys;
KeySet remainingKeys;
std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(),
marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end()));
if (debug)

2
gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
View File

@ -161,7 +161,7 @@ protected:
private:
/** Private methods for printing debug information */
static void PrintKeySet(const std::set<Key>& keys, const std::string& label);
static void PrintKeySet(const gtsam::FastSet<Key>& keys, const std::string& label);
static void PrintKeySet(const gtsam::KeySet& keys, const std::string& label);
static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor);
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label);

6
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -221,7 +221,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm
if(debug) { PrintNonlinearFactorGraph(smootherSummarization_, "ConcurrentBatchFilter::synchronize ", "Updated Smoother Summarization:", DefaultKeyFormatter); }
// Find the set of new separator keys
FastSet<Key> newSeparatorKeys;
KeySet newSeparatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
newSeparatorKeys.insert(key_value.key);
}
@ -573,7 +573,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
}
// Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove
FastSet<Key> newSeparatorKeys = removedFactors.keys();
KeySet newSeparatorKeys = removedFactors.keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
newSeparatorKeys.insert(key_value.key);
}
@ -584,7 +584,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
if(debug) { PrintKeys(newSeparatorKeys, "ConcurrentBatchFilter::synchronize ", "New Separator Keys:", DefaultKeyFormatter); }
// Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
FastSet<Key> shortcutKeys = newSeparatorKeys;
KeySet shortcutKeys = newSeparatorKeys;
BOOST_FOREACH(Key key, smootherSummarization_.keys()) {
shortcutKeys.insert(key);
}

2
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -371,7 +371,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() {
}
// Get the set of separator keys
gtsam::FastSet<Key> separatorKeys;
gtsam::KeySet separatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
separatorKeys.insert(key_value.key);
}

8
gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
View File

@ -52,16 +52,16 @@ namespace internal {
/* ************************************************************************* */
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
const FastSet<Key>& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) {
const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) {
// Calculate the set of RootKeys = AllKeys \Intersect RemainingKeys
FastSet<Key> rootKeys;
FastSet<Key> allKeys(graph.keys());
KeySet rootKeys;
KeySet allKeys(graph.keys());
std::set_intersection(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(rootKeys, rootKeys.end()));
// Calculate the set of MarginalizeKeys = AllKeys - RemainingKeys
FastSet<Key> marginalizeKeys;
KeySet marginalizeKeys;
std::set_difference(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(marginalizeKeys, marginalizeKeys.end()));
if(marginalizeKeys.size() == 0) {

2
gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h
View File

@ -152,7 +152,7 @@ namespace internal {
/** Calculate the marginal on the specified keys, returning a set of LinearContainerFactors.
* Unlike other GTSAM functions with similar purposes, this version can operate on disconnected graphs. */
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
const FastSet<Key>& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction);
const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction);
}

6
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -184,7 +184,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth
previousSmootherSummarization_ = smootherSummarization;
// Find the set of new separator keys
const FastSet<Key>& newSeparatorKeys = isam2_.getFixedVariables();
const KeySet& newSeparatorKeys = isam2_.getFixedVariables();
// Use the shortcut to calculate an updated marginal on the current separator
// Combine just the shortcut and the previousSmootherSummarization
@ -312,7 +312,7 @@ std::vector<size_t> ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2
void ConcurrentIncrementalFilter::updateShortcut(const NonlinearFactorGraph& removedFactors) {
// Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
FastSet<Key> shortcutKeys;
KeySet shortcutKeys;
BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) {
const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
if(factor) {
@ -343,7 +343,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
// variables that result from marginalizing out all of the other variables
// Find the set of current separator keys
const FastSet<Key>& separatorKeys = isam2_.getFixedVariables();
const KeySet& separatorKeys = isam2_.getFixedVariables();
// Find all cliques that contain any separator variables
std::set<ISAM2Clique::shared_ptr> separatorCliques;

2
gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
View File

@ -245,7 +245,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
}
// Get the set of separator keys
gtsam::FastSet<Key> separatorKeys;
gtsam::KeySet separatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
separatorKeys.insert(key_value.key);
}

2
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
View File

@ -559,7 +559,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 )
ordering = smoother.getOrdering(); // I'm really hoping this is an acceptable ordering...
GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues);
FastSet<Key> eliminateKeys = linearFactors->keys();
KeySet eliminateKeys = linearFactors->keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) {
eliminateKeys.erase(key_value.key);
}

2
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
View File

@ -579,7 +579,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
// GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph);
// GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
FastSet<Key> allkeys = LinFactorGraph->keys();
KeySet allkeys = LinFactorGraph->keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) {
allkeys.erase(key_value.key);
}

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

@ -581,7 +581,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
// GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph);
// GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
FastSet<Key> allkeys = LinFactorGraph->keys();
KeySet allkeys = LinFactorGraph->keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues)
allkeys.erase(key_value.key);
std::vector<Key> variables(allkeys.begin(), allkeys.end());

4
gtsam_unstable/slam/MultiProjectionFactor.h
View File

@ -70,7 +70,7 @@ namespace gtsam {
* @param body_P_sensor is the transform from body to sensor frame (default identity)
*/
MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
FastSet<Key> poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
KeySet poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
boost::optional<POSE> body_P_sensor = boost::none) :
Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(false), verboseCheirality_(false) {
@ -91,7 +91,7 @@ namespace gtsam {
* @param body_P_sensor is the transform from body to sensor frame (default identity)
*/
MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
FastSet<Key> poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
KeySet poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
bool throwCheirality, bool verboseCheirality,
boost::optional<POSE> body_P_sensor = boost::none) :
Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),

2
gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp
View File

@ -69,7 +69,7 @@ TEST( MultiProjectionFactor, create ){
n_measPixel << 10, 10, 10, 10, 10, 10;
const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
FastSet<Key> views;
KeySet views;
views.insert(x1);
views.insert(x2);
views.insert(x3);

8
matlab.h
View File

@ -71,16 +71,16 @@ FastVector<Key> createKeyVector(string s, const Vector& I) {
}
// Create a KeySet from indices
FastSet<Key> createKeySet(const Vector& I) {
FastSet<Key> set;
KeySet createKeySet(const Vector& I) {
KeySet set;
for (int i = 0; i < I.size(); i++)
set.insert(I[i]);
return set;
}
// Create a KeySet from indices using symbol
FastSet<Key> createKeySet(string s, const Vector& I) {
FastSet<Key> set;
KeySet createKeySet(string s, const Vector& I) {
KeySet set;
char c = s[0];
for (int i = 0; i < I.size(); i++)
set.insert(symbol(c, I[i]));

4
tests/testGaussianISAM2.cpp
View File

@ -186,12 +186,12 @@ done:
// Permuted<VectorValues> permuted(permutation, values);
//
// // After permutation, the indices above the threshold are 2 and 2
// FastSet<Key> expected;
// KeySet expected;
// expected.insert(2);
// expected.insert(3);
//
// // Indices checked by CheckRelinearization
// FastSet<Key> actual = Impl::CheckRelinearization(permuted, 0.1);
// KeySet actual = Impl::CheckRelinearization(permuted, 0.1);
//
// EXPECT(assert_equal(expected, actual));
//}

3
tests/testMarginals.cpp
View File

@ -226,7 +226,8 @@ TEST(Marginals, order) {
vals.at<Pose2>(3).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));
Marginals marginals(fg, vals);
FastVector<Key> keys(fg.keys());
KeySet set = fg.keys();
FastVector<Key> keys(set.begin(), set.end());
JointMarginal joint = marginals.jointMarginalCovariance(keys);
LONGS_EQUAL(3, (long)joint(0,0).rows());

4
tests/testNonlinearFactorGraph.cpp
View File

@ -66,9 +66,9 @@ TEST( NonlinearFactorGraph, error )
TEST( NonlinearFactorGraph, keys )
{
NonlinearFactorGraph fg = createNonlinearFactorGraph();
FastSet<Key> actual = fg.keys();
KeySet actual = fg.keys();
LONGS_EQUAL(3, (long)actual.size());
FastSet<Key>::const_iterator it = actual.begin();
KeySet::const_iterator it = actual.begin();
LONGS_EQUAL((long)L(1), (long)*(it++));
LONGS_EQUAL((long)X(1), (long)*(it++));
LONGS_EQUAL((long)X(2), (long)*(it++));