Merge remote-tracking branch 'origin/feature/reducedBloat' into feature/CeresDefaults
Frank Dellaert
commit
9359ad70e2
|
|
@ -158,6 +158,12 @@ else()
|
|||
set(GTSAM_USE_TBB 0) # This will go into config.h
|
||||
endif()
|
||||
|
||||
###############################################################################
|
||||
# Prohibit Timing build mode in combination with TBB
|
||||
if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing"))
|
||||
message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.")
|
||||
endif()
|
||||
|
||||
|
||||
###############################################################################
|
||||
# Find Google perftools
|
||||
|
|
|
|||
|
|
@ -19,15 +19,18 @@
|
|||
#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 <iosfwd>
|
||||
#include <iostream>
|
||||
#include <set>
|
||||
#include <string>
|
||||
|
||||
BOOST_MPL_HAS_XXX_TRAIT_DEF(print)
|
||||
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,152 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 <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>
|
||||
#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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -20,19 +20,15 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/dllexport.h>
|
||||
#include <gtsam/config.h>
|
||||
#include <boost/function/function1.hpp>
|
||||
#include <boost/concept/assert.hpp>
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/range/concepts.hpp>
|
||||
#include <boost/optional.hpp>
|
||||
#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
|
||||
|
|
@ -73,7 +69,6 @@ namespace gtsam {
|
|||
/// The index type for Eigen objects
|
||||
typedef ptrdiff_t DenseIndex;
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Helper class that uses templates to select between two types based on
|
||||
|
|
@ -148,104 +143,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
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
||||
|
|
|
|||
|
|
@ -18,6 +18,8 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
|
|||
|
|
@ -17,17 +17,18 @@
|
|||
|
||||
#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/FastVector.h>
|
||||
#include <gtsam/base/types.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/dllexport.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/none.hpp>
|
||||
#include <boost/optional/optional.hpp>
|
||||
#include <boost/smart_ptr/shared_ptr.hpp>
|
||||
|
||||
#include <vector>
|
||||
#include <deque>
|
||||
#include <cassert>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -123,26 +123,32 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
|
||||
// First find dimensions of each variable
|
||||
vector<size_t> dims;
|
||||
typedef std::map<Key, size_t> KeySizeMap;
|
||||
KeySizeMap dims;
|
||||
BOOST_FOREACH(const sharedFactor& factor, *this) {
|
||||
for (GaussianFactor::const_iterator pos = factor->begin();
|
||||
pos != factor->end(); ++pos) {
|
||||
if (dims.size() <= *pos)
|
||||
dims.resize(*pos + 1, 0);
|
||||
dims[*pos] = factor->getDim(pos);
|
||||
if (!static_cast<bool>(factor)) continue;
|
||||
|
||||
for (GaussianFactor::const_iterator key = factor->begin();
|
||||
key != factor->end(); ++key) {
|
||||
dims[*key] = factor->getDim(key);
|
||||
}
|
||||
}
|
||||
|
||||
// Compute first scalar column of each variable
|
||||
vector<size_t> columnIndices(dims.size() + 1, 0);
|
||||
for (size_t j = 1; j < dims.size() + 1; ++j)
|
||||
columnIndices[j] = columnIndices[j - 1] + dims[j - 1];
|
||||
size_t currentColIndex = 0;
|
||||
KeySizeMap columnIndices = dims;
|
||||
BOOST_FOREACH(const KeySizeMap::value_type& col, dims) {
|
||||
columnIndices[col.first] = currentColIndex;
|
||||
currentColIndex += dims[col.first];
|
||||
}
|
||||
|
||||
// Iterate over all factors, adding sparse scalar entries
|
||||
typedef boost::tuple<size_t, size_t, double> triplet;
|
||||
vector<triplet> entries;
|
||||
size_t row = 0;
|
||||
BOOST_FOREACH(const sharedFactor& factor, *this) {
|
||||
if (!static_cast<bool>(factor)) continue;
|
||||
|
||||
// Convert to JacobianFactor if necessary
|
||||
JacobianFactor::shared_ptr jacobianFactor(
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(factor));
|
||||
|
|
@ -159,11 +165,11 @@ namespace gtsam {
|
|||
// Whiten the factor and add entries for it
|
||||
// iterate over all variables in the factor
|
||||
const JacobianFactor whitened(jacobianFactor->whiten());
|
||||
for (JacobianFactor::const_iterator pos = whitened.begin();
|
||||
pos < whitened.end(); ++pos) {
|
||||
JacobianFactor::constABlock whitenedA = whitened.getA(pos);
|
||||
for (JacobianFactor::const_iterator key = whitened.begin();
|
||||
key < whitened.end(); ++key) {
|
||||
JacobianFactor::constABlock whitenedA = whitened.getA(key);
|
||||
// find first column index for this key
|
||||
size_t column_start = columnIndices[*pos];
|
||||
size_t column_start = columnIndices[*key];
|
||||
for (size_t i = 0; i < (size_t) whitenedA.rows(); i++)
|
||||
for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) {
|
||||
double s = whitenedA(i, j);
|
||||
|
|
@ -173,7 +179,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
JacobianFactor::constBVector whitenedb(whitened.getb());
|
||||
size_t bcolumn = columnIndices.back();
|
||||
size_t bcolumn = currentColIndex;
|
||||
for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
|
||||
entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i)));
|
||||
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
@ -268,18 +270,11 @@ void HessianFactor::print(const std::string& s,
|
|||
|
||||
/* ************************************************************************* */
|
||||
bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
|
||||
if (!dynamic_cast<const HessianFactor*>(&lf))
|
||||
const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&lf);
|
||||
if (!rhs || !Factor::equals(lf, tol))
|
||||
return false;
|
||||
else {
|
||||
if (!Factor::equals(lf, tol))
|
||||
return false;
|
||||
Matrix thisMatrix = info_.full().selfadjointView();
|
||||
thisMatrix(thisMatrix.rows() - 1, thisMatrix.cols() - 1) = 0.0;
|
||||
Matrix rhsMatrix =
|
||||
static_cast<const HessianFactor&>(lf).info_.full().selfadjointView();
|
||||
rhsMatrix(rhsMatrix.rows() - 1, rhsMatrix.cols() - 1) = 0.0;
|
||||
return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
|
||||
}
|
||||
return equal_with_abs_tol(augmentedInformation(), rhs->augmentedInformation(),
|
||||
tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -476,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());
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -66,10 +66,10 @@ JacobianFactor::JacobianFactor() :
|
|||
/* ************************************************************************* */
|
||||
JacobianFactor::JacobianFactor(const GaussianFactor& gf) {
|
||||
// Copy the matrix data depending on what type of factor we're copying from
|
||||
if (const JacobianFactor* rhs = dynamic_cast<const JacobianFactor*>(&gf))
|
||||
*this = JacobianFactor(*rhs);
|
||||
else if (const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&gf))
|
||||
*this = JacobianFactor(*rhs);
|
||||
if (const JacobianFactor* asJacobian = dynamic_cast<const JacobianFactor*>(&gf))
|
||||
*this = JacobianFactor(*asJacobian);
|
||||
else if (const HessianFactor* asHessian = dynamic_cast<const HessianFactor*>(&gf))
|
||||
*this = JacobianFactor(*asHessian);
|
||||
else
|
||||
throw std::invalid_argument(
|
||||
"In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
|
||||
|
|
@ -432,8 +432,6 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
double JacobianFactor::error(const VectorValues& c) const {
|
||||
if (empty())
|
||||
return 0;
|
||||
Vector weighted = error_vector(c);
|
||||
return 0.5 * weighted.dot(weighted);
|
||||
}
|
||||
|
|
@ -729,8 +727,8 @@ std::pair<boost::shared_ptr<GaussianConditional>,
|
|||
jointFactor->Ab_.matrix().triangularView<Eigen::StrictlyLower>().setZero();
|
||||
|
||||
// Split elimination result into conditional and remaining factor
|
||||
GaussianConditional::shared_ptr conditional = jointFactor->splitConditional(
|
||||
keys.size());
|
||||
GaussianConditional::shared_ptr conditional = //
|
||||
jointFactor->splitConditional(keys.size());
|
||||
|
||||
return make_pair(conditional, jointFactor);
|
||||
}
|
||||
|
|
@ -759,11 +757,11 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(
|
|||
}
|
||||
GaussianConditional::shared_ptr conditional = boost::make_shared<
|
||||
GaussianConditional>(Base::keys_, nrFrontals, Ab_, conditionalNoiseModel);
|
||||
const DenseIndex maxRemainingRows = std::min(Ab_.cols() - 1, originalRowEnd)
|
||||
const DenseIndex maxRemainingRows = std::min(Ab_.cols(), originalRowEnd)
|
||||
- Ab_.rowStart() - frontalDim;
|
||||
const DenseIndex remainingRows =
|
||||
model_ ?
|
||||
std::min(model_->sigmas().size() - frontalDim, maxRemainingRows) :
|
||||
model_ ? std::min(model_->sigmas().size() - frontalDim,
|
||||
maxRemainingRows) :
|
||||
maxRemainingRows;
|
||||
Ab_.rowStart() += frontalDim;
|
||||
Ab_.rowEnd() = Ab_.rowStart() + remainingRows;
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
||||
|
|
|
|||
|
|
@ -280,30 +280,69 @@ TEST(HessianFactor, ConstructorNWay)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HessianFactor, CombineAndEliminate)
|
||||
{
|
||||
Matrix A01 = (Matrix(3,3) <<
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0).finished();
|
||||
TEST(HessianFactor, CombineAndEliminate1) {
|
||||
Matrix3 A01 = 3.0 * I_3x3;
|
||||
Vector3 b0(1, 0, 0);
|
||||
|
||||
Matrix3 A21 = 4.0 * I_3x3;
|
||||
Vector3 b2 = Vector3::Zero();
|
||||
|
||||
GaussianFactorGraph gfg;
|
||||
gfg.add(1, A01, b0);
|
||||
gfg.add(1, A21, b2);
|
||||
|
||||
Matrix63 A1;
|
||||
A1 << A01, A21;
|
||||
Vector6 b;
|
||||
b << b0, b2;
|
||||
|
||||
// create a full, uneliminated version of the factor
|
||||
JacobianFactor jacobian(1, A1, b);
|
||||
|
||||
// Make sure combining works
|
||||
HessianFactor hessian(gfg);
|
||||
VectorValues v;
|
||||
v.insert(1, Vector3(1, 0, 0));
|
||||
EXPECT_DOUBLES_EQUAL(jacobian.error(v), hessian.error(v), 1e-9);
|
||||
EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6));
|
||||
EXPECT(assert_equal(25.0 * I_3x3, hessian.information(), 1e-9));
|
||||
EXPECT(
|
||||
assert_equal(jacobian.augmentedInformation(),
|
||||
hessian.augmentedInformation(), 1e-9));
|
||||
|
||||
// perform elimination on jacobian
|
||||
Ordering ordering = list_of(1);
|
||||
GaussianConditional::shared_ptr expectedConditional;
|
||||
JacobianFactor::shared_ptr expectedFactor;
|
||||
boost::tie(expectedConditional, expectedFactor) = //
|
||||
jacobian.eliminate(ordering);
|
||||
|
||||
// Eliminate
|
||||
GaussianConditional::shared_ptr actualConditional;
|
||||
HessianFactor::shared_ptr actualHessian;
|
||||
boost::tie(actualConditional, actualHessian) = //
|
||||
EliminateCholesky(gfg, ordering);
|
||||
|
||||
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
|
||||
EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
|
||||
EXPECT(
|
||||
assert_equal(expectedFactor->augmentedInformation(),
|
||||
actualHessian->augmentedInformation(), 1e-9));
|
||||
EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(HessianFactor, CombineAndEliminate2) {
|
||||
Matrix A01 = I_3x3;
|
||||
Vector3 b0(1.5, 1.5, 1.5);
|
||||
Vector3 s0(1.6, 1.6, 1.6);
|
||||
|
||||
Matrix A10 = (Matrix(3,3) <<
|
||||
2.0, 0.0, 0.0,
|
||||
0.0, 2.0, 0.0,
|
||||
0.0, 0.0, 2.0).finished();
|
||||
Matrix A11 = (Matrix(3,3) <<
|
||||
-2.0, 0.0, 0.0,
|
||||
0.0, -2.0, 0.0,
|
||||
0.0, 0.0, -2.0).finished();
|
||||
Matrix A10 = 2.0 * I_3x3;
|
||||
Matrix A11 = -2.0 * I_3x3;
|
||||
Vector3 b1(2.5, 2.5, 2.5);
|
||||
Vector3 s1(2.6, 2.6, 2.6);
|
||||
|
||||
Matrix A21 = (Matrix(3,3) <<
|
||||
3.0, 0.0, 0.0,
|
||||
0.0, 3.0, 0.0,
|
||||
0.0, 0.0, 3.0).finished();
|
||||
Matrix A21 = 3.0 * I_3x3;
|
||||
Vector3 b2(3.5, 3.5, 3.5);
|
||||
Vector3 s2(3.6, 3.6, 3.6);
|
||||
|
||||
|
|
@ -312,29 +351,45 @@ TEST(HessianFactor, CombineAndEliminate)
|
|||
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
|
||||
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
|
||||
|
||||
Matrix93 A0; A0 << A10, Z_3x3, Z_3x3;
|
||||
Matrix93 A1; A1 << A11, A01, A21;
|
||||
Vector9 b; b << b1, b0, b2;
|
||||
Vector9 sigmas; sigmas << s1, s0, s2;
|
||||
Matrix93 A0, A1;
|
||||
A0 << A10, Z_3x3, Z_3x3;
|
||||
A1 << A11, A01, A21;
|
||||
Vector9 b, sigmas;
|
||||
b << b1, b0, b2;
|
||||
sigmas << s1, s0, s2;
|
||||
|
||||
// create a full, uneliminated version of the factor
|
||||
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
|
||||
JacobianFactor jacobian(0, A0, 1, A1, b,
|
||||
noiseModel::Diagonal::Sigmas(sigmas, true));
|
||||
|
||||
// Make sure combining works
|
||||
EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6));
|
||||
HessianFactor hessian(gfg);
|
||||
EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6));
|
||||
EXPECT(
|
||||
assert_equal(jacobian.augmentedInformation(),
|
||||
hessian.augmentedInformation(), 1e-9));
|
||||
|
||||
// perform elimination on jacobian
|
||||
Ordering ordering = list_of(0);
|
||||
GaussianConditional::shared_ptr expectedConditional;
|
||||
JacobianFactor::shared_ptr expectedRemainingFactor;
|
||||
boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0)));
|
||||
JacobianFactor::shared_ptr expectedFactor;
|
||||
boost::tie(expectedConditional, expectedFactor) = //
|
||||
jacobian.eliminate(ordering);
|
||||
|
||||
// Eliminate
|
||||
GaussianConditional::shared_ptr actualConditional;
|
||||
HessianFactor::shared_ptr actualCholeskyFactor;
|
||||
boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0)));
|
||||
HessianFactor::shared_ptr actualHessian;
|
||||
boost::tie(actualConditional, actualHessian) = //
|
||||
EliminateCholesky(gfg, ordering);
|
||||
|
||||
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
|
||||
EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6));
|
||||
VectorValues v;
|
||||
v.insert(1, Vector3(1, 2, 3));
|
||||
EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
|
||||
EXPECT(
|
||||
assert_equal(expectedFactor->augmentedInformation(),
|
||||
actualHessian->augmentedInformation(), 1e-9));
|
||||
EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -540,9 +540,9 @@ TEST(JacobianFactor, EliminateQR)
|
|||
EXPECT(assert_equal(size_t(2), actualJF.keys().size()));
|
||||
EXPECT(assert_equal(Key(9), actualJF.keys()[0]));
|
||||
EXPECT(assert_equal(Key(11), actualJF.keys()[1]));
|
||||
EXPECT(assert_equal(Matrix(R.block(6, 6, 4, 2)), actualJF.getA(actualJF.begin()), 0.001));
|
||||
EXPECT(assert_equal(Matrix(R.block(6, 8, 4, 2)), actualJF.getA(actualJF.begin()+1), 0.001));
|
||||
EXPECT(assert_equal(Vector(R.col(10).segment(6, 4)), actualJF.getb(), 0.001));
|
||||
EXPECT(assert_equal(Matrix(R.block(6, 6, 5, 2)), actualJF.getA(actualJF.begin()), 0.001));
|
||||
EXPECT(assert_equal(Matrix(R.block(6, 8, 5, 2)), actualJF.getA(actualJF.begin()+1), 0.001));
|
||||
EXPECT(assert_equal(Vector(R.col(10).segment(6, 5)), actualJF.getb(), 0.001));
|
||||
EXPECT(!actualJF.get_model());
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <gtsam/slam/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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -643,7 +643,6 @@ public:
|
|||
// gs = F' * (b - E * P * E' * b)
|
||||
|
||||
MatrixDD matrixBlock;
|
||||
typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
|
||||
|
||||
FastMap<Key, size_t> KeySlotMap;
|
||||
for (size_t slot = 0; slot < allKeys.size(); slot++)
|
||||
|
|
|
|||
|
|
@ -16,6 +16,7 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/slam/RegularHessianFactor.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -29,30 +30,58 @@ 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
|
||||
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();
|
||||
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;
|
||||
|
||||
Matrix G33 = I_2x2;
|
||||
|
||||
Vector2 g1 = b, g2 = b, g3 = b;
|
||||
|
||||
double f = 10;
|
||||
|
||||
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);
|
||||
// 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 +90,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 +106,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));
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
FastSet<Key> set = fg.keys();
|
||||
FastVector<Key> keys(set.begin(), set.end());
|
||||
JointMarginal joint = marginals.jointMarginalCovariance(keys);
|
||||
|
||||
LONGS_EQUAL(3, (long)joint(0,0).rows());
|
||||
|
|
|
|||
Loading…
Reference in New Issue