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

sync with develop branch commit c1f048d

release/4.3a0
krunalchande 2014-12-12 18:46:54 -05:00
parent 9df5ce9732
commit da318184ae
24 changed files with 636 additions and 731 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -1,4 +1,5 @@
/build* /build*
/doc*
*.pyc *.pyc
*.DS_Store *.DS_Store
/examples/Data/dubrovnik-3-7-pre-rewritten.txt /examples/Data/dubrovnik-3-7-pre-rewritten.txt

2
cmake/Config.cmake.in
View File

@ -6,7 +6,7 @@
get_filename_component(OUR_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) get_filename_component(OUR_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
if(EXISTS "${OUR_CMAKE_DIR}/CMakeCache.txt") if(EXISTS "${OUR_CMAKE_DIR}/CMakeCache.txt")
# In build tree # In build tree
set(@PACKAGE_NAME@_INCLUDE_DIR @CMAKE_SOURCE_DIR@ @CMAKE_BINARY_DIR@ CACHE PATH "@PACKAGE_NAME@ include directory") set(@PACKAGE_NAME@_INCLUDE_DIR @CMAKE_SOURCE_DIR@ CACHE PATH "@PACKAGE_NAME@ include directory")
else() else()
# Find installed library # Find installed library
set(@PACKAGE_NAME@_INCLUDE_DIR "${OUR_CMAKE_DIR}/@CONF_REL_INCLUDE_DIR@" CACHE PATH "@PACKAGE_NAME@ include directory") set(@PACKAGE_NAME@_INCLUDE_DIR "${OUR_CMAKE_DIR}/@CONF_REL_INCLUDE_DIR@" CACHE PATH "@PACKAGE_NAME@ include directory")

10
gtsam.h
View File

@ -1250,14 +1250,6 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
const gtsam::noiseModel::Diagonal* model); const gtsam::noiseModel::Diagonal* model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3,
Vector b, const gtsam::noiseModel::Diagonal* model); Vector b, const gtsam::noiseModel::Diagonal* model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3,
size_t i4, Matrix A4, Vector b, const gtsam::noiseModel::Diagonal* model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3,
size_t i4, Matrix A4, size_t i5, Matrix A5, Vector b,
const gtsam::noiseModel::Diagonal* model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3,
size_t i4, Matrix A4, size_t i5, Matrix A5, size_t i6, Matrix A6, Vector b,
const gtsam::noiseModel::Diagonal* model);
JacobianFactor(const gtsam::GaussianFactorGraph& graph); JacobianFactor(const gtsam::GaussianFactorGraph& graph);
//Testable //Testable
@ -1664,7 +1656,6 @@ class NonlinearFactorGraph {
gtsam::Ordering orderingCOLAMD() const; gtsam::Ordering orderingCOLAMD() const;
// Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map<gtsam::Key,int>& constraints) const; // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map<gtsam::Key,int>& constraints) const;
gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const;
gtsam::GaussianFactorGraph* multipliedHessians(const gtsam::Values& values, const gtsam::VectorValues& duals) const;
gtsam::NonlinearFactorGraph clone() const; gtsam::NonlinearFactorGraph clone() const;
// enabling serialization functionality // enabling serialization functionality
@ -1675,7 +1666,6 @@ virtual class NonlinearFactor {
// Factor base class // Factor base class
size_t size() const; size_t size() const;
gtsam::KeyVector keys() const; gtsam::KeyVector keys() const;
size_t dualKey() const;
void print(string s) const; void print(string s) const;
void printKeys(string s) const; void printKeys(string s) const;
// NonlinearFactor // NonlinearFactor

15
gtsam/3rdparty/metis/CMakeLists.txt vendored
View File

@ -4,11 +4,20 @@ project(METIS)
# Add flags for currect directory and below # Add flags for currect directory and below
if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
add_definitions(-Wno-unused-variable) add_definitions(-Wno-unused-variable)
# add_definitions(-Wno-sometimes-uninitialized) if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 5.0 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 5.0)
add_definitions(-Wno-sometimes-uninitialized)
endif()
endif() endif()
add_definitions(-Wno-unknown-pragmas) if(NOT ("${CMAKE_C_COMPILER_ID}" MATCHES "MSVC" OR "${CMAKE_CXX_COMPILER_ID}" MATCHES "MSVC"))
#add_definitions(-Wunused-but-set-variable) #add_definitions(-Wno-unknown-pragmas)
endif()
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 4.6 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 4.6)
add_definitions(-Wno-unused-but-set-variable)
endif()
endif()
set(GKLIB_PATH ${PROJECT_SOURCE_DIR}/GKlib CACHE PATH "path to GKlib") set(GKLIB_PATH ${PROJECT_SOURCE_DIR}/GKlib CACHE PATH "path to GKlib")
set(SHARED FALSE CACHE BOOL "build a shared library") set(SHARED FALSE CACHE BOOL "build a shared library")

3
gtsam/3rdparty/metis/GKlib/gk_arch.h vendored
View File

@ -59,9 +59,10 @@ typedef ptrdiff_t ssize_t;
#endif #endif
#ifdef __MSC__ #ifdef __MSC__
#if(_MSC_VER < 1700)
/* MSC does not have rint() function */ /* MSC does not have rint() function */
#define rint(x) ((int)((x)+0.5)) #define rint(x) ((int)((x)+0.5))
#endif
/* MSC does not have INFINITY defined */ /* MSC does not have INFINITY defined */
#ifndef INFINITY #ifndef INFINITY
#define INFINITY FLT_MAX #define INFINITY FLT_MAX

3
gtsam/base/Matrix.cpp
View File

@ -543,8 +543,7 @@ Matrix collect(size_t nrMatrices, ...)
void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) { void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) {
const DenseIndex m = A.rows(); const DenseIndex m = A.rows();
if (inf_mask) { if (inf_mask) {
// only scale the first v.size() rows of A to support augmented Matrix for (DenseIndex i=0; i<m; ++i) {
for (DenseIndex i=0; i<v.size(); ++i) {
const double& vi = v(i); const double& vi = v(i);
if (std::isfinite(vi)) if (std::isfinite(vi))
A.row(i) *= vi; A.row(i) *= vi;

1
gtsam/base/Matrix.h
View File

@ -463,7 +463,6 @@ GTSAM_EXPORT Matrix collect(size_t nrMatrices, ...);
* Arguments (Matrix, Vector) scales the columns, * Arguments (Matrix, Vector) scales the columns,
* (Vector, Matrix) scales the rows * (Vector, Matrix) scales the rows
* @param inf_mask when true, will not scale with a NaN or inf value. * @param inf_mask when true, will not scale with a NaN or inf value.
* The inplace version also allows v.size()<A.rows() and only scales the first v.size() rows of A.
*/ */
GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row

2
gtsam/base/Testable.h
View File

@ -86,7 +86,7 @@ namespace gtsam {
* This template works for any type with equals * This template works for any type with equals
*/ */
template<class V> template<class V>
bool assert_equal(const V& expected, const V& actual, double tol = 1e-8) { bool assert_equal(const V& expected, const V& actual, double tol = 1e-9) {
if (actual.equals(expected, tol)) if (actual.equals(expected, tol))
return true; return true;
printf("Not equal:\n"); printf("Not equal:\n");

11
gtsam/base/Vector.cpp
View File

@ -281,17 +281,12 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights,
size_t m = weights.size(); size_t m = weights.size();
static const double inf = std::numeric_limits<double>::infinity(); static const double inf = std::numeric_limits<double>::infinity();
// Check once for zero entries of a. // Check once for zero entries of a. TODO: really needed ?
vector<bool> isZero; vector<bool> isZero;
for (size_t i = 0; i < m; ++i) { for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9);
isZero.push_back(fabs(a[i]) < 1e-9);
// If there is a valid (a[i]!=0) inequality constraint (weight<0),
// ignore it by also setting isZero flag
if (!isZero[i] && (weights[i]<0)) isZero[i] = true;
}
// If there is a valid (a!=0) constraint (sigma==0) return the first one
for (size_t i = 0; i < m; ++i) { for (size_t i = 0; i < m; ++i) {
// If there is a valid (a!=0) constraint (sigma==0) return the first one
if (weights[i] == inf && !isZero[i]) { if (weights[i] == inf && !isZero[i]) {
// Basically, instead of doing a normal QR step with the weighted // Basically, instead of doing a normal QR step with the weighted
// pseudoinverse, we enforce the constraint by turning // pseudoinverse, we enforce the constraint by turning

2
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -300,7 +300,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues GaussianFactorGraph::gradientAtZero() const { VectorValues GaussianFactorGraph::gradientAtZero() const {
// Zero-out the gradient // Zero-out the gradient
VectorValues g; VectorValues g;
BOOST_FOREACH(const sharedFactor& factor, *this) { BOOST_FOREACH(const sharedFactor& factor, *this) {

7
gtsam/linear/GaussianFactorGraph.h
View File

@ -262,6 +262,7 @@ namespace gtsam {
/** /**
* Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b * Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b
* \right\Vert^2 \f$, centered around zero. The gradient is \f$ A^T(Ax-b) \f$. * \right\Vert^2 \f$, centered around zero. The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @param [output] g A VectorValues to store the gradient, which must be preallocated, * @param [output] g A VectorValues to store the gradient, which must be preallocated,
* see allocateVectorValues * see allocateVectorValues
* @return The gradient as a VectorValues */ * @return The gradient as a VectorValues */
@ -321,12 +322,6 @@ namespace gtsam {
/// @} /// @}
/**
* Split constraints and unconstrained factors into two different graphs
* @return a pair of <unconstrained, constrained> graphs
*/
boost::tuple<GaussianFactorGraph, GaussianFactorGraph, GaussianFactorGraph> splitConstraints() const;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;

2
gtsam/linear/HessianFactor.cpp
View File

@ -650,7 +650,6 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
HessianFactor::shared_ptr jointFactor; HessianFactor::shared_ptr jointFactor;
try { try {
jointFactor = boost::make_shared<HessianFactor>(factors, Scatter(factors, keys)); jointFactor = boost::make_shared<HessianFactor>(factors, Scatter(factors, keys));
// jointFactor->print("jointFactor: ");
} catch(std::invalid_argument&) { } catch(std::invalid_argument&) {
throw InvalidDenseElimination( throw InvalidDenseElimination(
"EliminateCholesky was called with a request to eliminate variables that are not\n" "EliminateCholesky was called with a request to eliminate variables that are not\n"
@ -666,7 +665,6 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
// Erase the eliminated keys in the remaining factor // Erase the eliminated keys in the remaining factor
jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate); jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate);
} catch(CholeskyFailed&) { } catch(CholeskyFailed&) {
// std::cout << "Problematic Hessian: " << jointFactor->information() << std::endl;
throw IndeterminantLinearSystemException(keys.front()); throw IndeterminantLinearSystemException(keys.front());
} }

5
gtsam/linear/HessianFactor.h
View File

@ -384,10 +384,7 @@ namespace gtsam {
void multiplyHessianAdd(double alpha, const double* x, double* y) const {}; void multiplyHessianAdd(double alpha, const double* x, double* y) const {};
/** /// eta for Hessian
* eta for Hessian
* Ignore duals parameters. It's only valid for constraints, which need to be a JacobianFactor
*/
VectorValues gradientAtZero() const; VectorValues gradientAtZero() const;
virtual void gradientAtZero(double* d) const; virtual void gradientAtZero(double* d) const;

164
gtsam/linear/JacobianFactor-inl.h
View File

@ -19,98 +19,90 @@
#pragma once #pragma once
#include <gtsam/linear/linearExceptions.h> #include <gtsam/linear/linearExceptions.h>
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/algorithm/for_each.hpp>
#include <boost/foreach.hpp>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<typename TERMS> template<typename TERMS>
JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, const SharedDiagonal& model)
const SharedDiagonal& model) { {
fillTerms(terms, b, model); fillTerms(terms, b, model);
}
/* ************************************************************************* */
template<typename KEYS>
JacobianFactor::JacobianFactor(const KEYS& keys,
const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) :
Base(keys), Ab_(augmentedMatrix) {
// Check noise model dimension
if (model && (DenseIndex) model->dim() != augmentedMatrix.rows())
throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim());
// Check number of variables
if ((DenseIndex) Base::keys_.size() != augmentedMatrix.nBlocks() - 1)
throw std::invalid_argument(
"Error in JacobianFactor constructor input. Number of provided keys plus\n"
"one for the RHS vector must equal the number of provided matrix blocks.");
// Check RHS dimension
if (augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1)
throw std::invalid_argument(
"Error in JacobianFactor constructor input. The last provided matrix block\n"
"must be the RHS vector, but the last provided block had more than one column.");
// Take noise model
model_ = model;
}
/* ************************************************************************* */
namespace internal {
static inline DenseIndex getColsJF(const std::pair<Key, Matrix>& p) {
return p.second.cols();
}
}
/* ************************************************************************* */
template<typename TERMS>
void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b,
const SharedDiagonal& noiseModel) {
using boost::adaptors::transformed;
namespace br = boost::range;
// Check noise model dimension
if (noiseModel && (DenseIndex) noiseModel->dim() != b.size())
throw InvalidNoiseModel(b.size(), noiseModel->dim());
// Resize base class key vector
Base::keys_.resize(terms.size());
// Construct block matrix - this uses the boost::range 'transformed' construct to apply
// internal::getColsJF (defined just above here in this file) to each term. This
// transforms the list of terms into a list of variable dimensions, by extracting the
// number of columns in each matrix. This is done to avoid separately allocating an
// array of dimensions before constructing the VerticalBlockMatrix.
Ab_ = VerticalBlockMatrix(terms | transformed(&internal::getColsJF), b.size(),
true);
// Check and add terms
DenseIndex i = 0; // For block index
for (typename TERMS::const_iterator termIt = terms.begin();
termIt != terms.end(); ++termIt) {
const std::pair<Key, Matrix>& term = *termIt;
// Check block rows
if (term.second.rows() != Ab_.rows())
throw InvalidMatrixBlock(Ab_.rows(), term.second.rows());
// Assign key and matrix
Base::keys_[i] = term.first;
Ab_(i) = term.second;
// Increment block index
++i;
} }
// Assign RHS vector /* ************************************************************************* */
getb() = b; template<typename KEYS>
JacobianFactor::JacobianFactor(
const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) :
Base(keys), Ab_(augmentedMatrix)
{
// Check noise model dimension
if(model && (DenseIndex)model->dim() != augmentedMatrix.rows())
throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim());
// Assign noise model // Check number of variables
model_ = noiseModel; if((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1)
} throw std::invalid_argument(
"Error in JacobianFactor constructor input. Number of provided keys plus\n"
"one for the RHS vector must equal the number of provided matrix blocks.");
// Check RHS dimension
if(augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1)
throw std::invalid_argument(
"Error in JacobianFactor constructor input. The last provided matrix block\n"
"must be the RHS vector, but the last provided block had more than one column.");
// Take noise model
model_ = model;
}
/* ************************************************************************* */
template<typename TERMS>
void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel)
{
// Check noise model dimension
if(noiseModel && (DenseIndex)noiseModel->dim() != b.size())
throw InvalidNoiseModel(b.size(), noiseModel->dim());
// Resize base class key vector
Base::keys_.resize(terms.size());
// Get dimensions of matrices
std::vector<size_t> dimensions;
dimensions.reserve(terms.size());
for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) {
const std::pair<Key, Matrix>& term = *it;
const Matrix& Ai = term.second;
dimensions.push_back(Ai.cols());
}
// Construct block matrix
Ab_ = VerticalBlockMatrix(dimensions, b.size(), true);
// Check and add terms
DenseIndex i = 0; // For block index
for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) {
const std::pair<Key, Matrix>& term = *it;
Key key = term.first;
const Matrix& Ai = term.second;
// Check block rows
if(Ai.rows() != Ab_.rows())
throw InvalidMatrixBlock(Ab_.rows(), Ai.rows());
// Assign key and matrix
Base::keys_[i] = key;
Ab_(i) = Ai;
// Increment block index
++ i;
}
// Assign RHS vector
getb() = b;
// Assign noise model
model_ = noiseModel;
}
} // gtsam } // gtsam

25
gtsam/linear/VectorValues.cpp
View File

@ -66,15 +66,6 @@ namespace gtsam {
return result; return result;
} }
/* ************************************************************************* */
VectorValues VectorValues::One(const VectorValues& other)
{
VectorValues result;
BOOST_FOREACH(const KeyValuePair& v, other)
result.values_.insert(make_pair(v.first, Vector::Ones(v.second.size())));
return result;
}
/* ************************************************************************* */ /* ************************************************************************* */
void VectorValues::update(const VectorValues& values) void VectorValues::update(const VectorValues& values)
{ {
@ -316,22 +307,6 @@ namespace gtsam {
return result; return result;
} }
/* ************************************************************************* */
VectorValues VectorValues::operator*(const VectorValues& c) const
{
if(this->size() != c.size())
throw invalid_argument("VectorValues::operator* called with different vector sizes");
assert_throw(hasSameStructure(c),
invalid_argument("VectorValues::operator* called with different vector sizes"));
VectorValues result;
// The result.end() hint here should result in constant-time inserts
for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
result.values_.insert(result.end(), make_pair(j1->first, j1->second * j2->second));
return result;
}
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues VectorValues::subtract(const VectorValues& c) const VectorValues VectorValues::subtract(const VectorValues& c) const
{ {

9
gtsam/linear/VectorValues.h
View File

@ -130,15 +130,12 @@ namespace gtsam {
/** Create a VectorValues with the same structure as \c other, but filled with zeros. */ /** Create a VectorValues with the same structure as \c other, but filled with zeros. */
static VectorValues Zero(const VectorValues& other); static VectorValues Zero(const VectorValues& other);
/** Create a VectorValues with the same structure as \c other, but filled with a constant. */
static VectorValues One(const VectorValues& other);
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/** Number of variables stored. */ /** Number of variables stored. */
size_t size() const { return values_.size(); } Key size() const { return values_.size(); }
/** Return the dimension of variable \c j. */ /** Return the dimension of variable \c j. */
size_t dim(Key j) const { return at(j).rows(); } size_t dim(Key j) const { return at(j).rows(); }
@ -305,10 +302,6 @@ namespace gtsam {
* structure (checked when NDEBUG is not defined). */ * structure (checked when NDEBUG is not defined). */
VectorValues subtract(const VectorValues& c) const; VectorValues subtract(const VectorValues& c) const;
/** Element-wise multiplication. Both VectorValues must have the same structure
* (checked when NDEBUG is not defined). */
VectorValues operator*(const VectorValues& c) const;
/** Element-wise scaling by a constant. */ /** Element-wise scaling by a constant. */
friend GTSAM_EXPORT VectorValues operator*(const double a, const VectorValues &v); friend GTSAM_EXPORT VectorValues operator*(const double a, const VectorValues &v);

20
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -105,6 +105,24 @@ TEST(JacobianFactor, constructors_and_accessors)
EXPECT(noise == expected.get_model()); EXPECT(noise == expected.get_model());
EXPECT(noise == actual.get_model()); EXPECT(noise == actual.get_model());
} }
{
// Test three-term constructor with std::map
JacobianFactor expected(
boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, noise);
map<Key,Matrix> mapTerms;
// note order of insertion plays no role: order will be determined by keys
mapTerms.insert(terms[2]);
mapTerms.insert(terms[1]);
mapTerms.insert(terms[0]);
JacobianFactor actual(mapTerms, b, noise);
EXPECT(assert_equal(expected, actual));
LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
EXPECT(assert_equal(b, expected.getb()));
EXPECT(assert_equal(b, actual.getb()));
EXPECT(noise == expected.get_model());
EXPECT(noise == actual.get_model());
}
{ {
// VerticalBlockMatrix constructor // VerticalBlockMatrix constructor
JacobianFactor expected( JacobianFactor expected(
@ -576,7 +594,7 @@ TEST ( JacobianFactor, constraint_eliminate2 )
Matrix m(1,2); Matrix m(1,2);
Matrix Aempty = m.topRows(0); Matrix Aempty = m.topRows(0);
Vector bempty = m.block(0,0,0,1); Vector bempty = m.block(0,0,0,1);
JacobianFactor expectedLF(2, Aempty, bempty, noiseModel::Diagonal::Sigmas(Vector())); JacobianFactor expectedLF(2, Aempty, bempty, noiseModel::Constrained::All(0));
EXPECT(assert_equal(expectedLF, *actual.second)); EXPECT(assert_equal(expectedLF, *actual.second));
// verify CG // verify CG

9
gtsam_unstable/gtsam_unstable.h
View File

@ -48,15 +48,6 @@ class Dummy {
unsigned char dummyTwoVar(unsigned char a) const; unsigned char dummyTwoVar(unsigned char a) const;
}; };
#include <gtsam_unstable/linear/QPSolver.h>
class QPSolver {
QPSolver(const gtsam::GaussianFactorGraph &graph);
pair<gtsam::VectorValues, gtsam::VectorValues> optimize(const gtsam::VectorValues& initials) const;
pair<gtsam::VectorValues, gtsam::VectorValues> optimize() const;
pair<bool, gtsam::VectorValues> findFeasibleInitialValues() const;
};
#include <gtsam_unstable/dynamics/PoseRTV.h> #include <gtsam_unstable/dynamics/PoseRTV.h>
class PoseRTV { class PoseRTV {
PoseRTV(); PoseRTV();

906
gtsam_unstable/partition/FindSeparator-inl.h
View File

@ -28,539 +28,539 @@ extern "C" {
namespace gtsam { namespace partition { namespace gtsam { namespace partition {
typedef boost::shared_array<idx_t> sharedInts; typedef boost::shared_array<idx_t> sharedInts;
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Return the size of the separator and the partiion indices {part} * Return the size of the separator and the partiion indices {part}
* Part [j] is 0, 1, or 2, depending on * Part [j] is 0, 1, or 2, depending on
* whether node j is in the left part of the graph, the right part, or the * whether node j is in the left part of the graph, the right part, or the
* separator, respectively * separator, respectively
*/ */
std::pair<int, sharedInts> separatorMetis(idx_t n, const sharedInts& xadj, std::pair<int, sharedInts> separatorMetis(idx_t n, const sharedInts& xadj,
const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) { const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) {
// control parameters // control parameters
idx_t vwgt[n]; // the weights of the vertices idx_t vwgt[n]; // the weights of the vertices
idx_t options[METIS_NOPTIONS]; idx_t options[METIS_NOPTIONS];
METIS_SetDefaultOptions(options); // use defaults METIS_SetDefaultOptions(options); // use defaults
idx_t sepsize; // the size of the separator, output idx_t sepsize; // the size of the separator, output
sharedInts part_(new idx_t[n]); // the partition of each vertex, output sharedInts part_(new idx_t[n]); // the partition of each vertex, output
// set uniform weights on the vertices // set uniform weights on the vertices
std::fill(vwgt, vwgt+n, 1); std::fill(vwgt, vwgt+n, 1);
// TODO: Fix at later time // TODO: Fix at later time
//boost::timer::cpu_timer TOTALTmr; //boost::timer::cpu_timer TOTALTmr;
if (verbose) { if (verbose) {
printf("**********************************************************************\n"); printf("**********************************************************************\n");
printf("Graph Information ---------------------------------------------------\n"); printf("Graph Information ---------------------------------------------------\n");
printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2); printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2);
printf("\nND Partitioning... -------------------------------------------\n"); printf("\nND Partitioning... -------------------------------------------\n");
//TOTALTmr.start() //TOTALTmr.start()
} }
// call metis parition routine // call metis parition routine
METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(), METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(),
vwgt, options, &sepsize, part_.get()); vwgt, options, &sepsize, part_.get());
if (verbose) { if (verbose) {
//boost::cpu_times const elapsed_times(timer.elapsed()); //boost::cpu_times const elapsed_times(timer.elapsed());
//printf("\nTiming Information --------------------------------------------------\n"); //printf("\nTiming Information --------------------------------------------------\n");
//printf(" Total: \t\t %7.3f\n", elapsed_times); //printf(" Total: \t\t %7.3f\n", elapsed_times);
printf(" Sep size: \t\t %d\n", sepsize); printf(" Sep size: \t\t %d\n", sepsize);
printf("**********************************************************************\n"); printf("**********************************************************************\n");
} }
return std::make_pair(sepsize, part_); return std::make_pair(sepsize, part_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void modefied_EdgeComputeSeparator(idx_t *nvtxs, idx_t *xadj, idx_t *adjncy, idx_t *vwgt, void modefied_EdgeComputeSeparator(idx_t *nvtxs, idx_t *xadj, idx_t *adjncy, idx_t *vwgt,
idx_t *adjwgt, idx_t *options, idx_t *edgecut, idx_t *part) idx_t *adjwgt, idx_t *options, idx_t *edgecut, idx_t *part)
{ {
idx_t i, ncon; idx_t i, ncon;
graph_t *graph; graph_t *graph;
real_t *tpwgts2; real_t *tpwgts2;
ctrl_t *ctrl; ctrl_t *ctrl;
ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL); ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL);
ctrl->iptype = METIS_IPTYPE_GROW; ctrl->iptype = METIS_IPTYPE_GROW;
//if () == NULL) //if () == NULL)
// return METIS_ERROR_INPUT; // return METIS_ERROR_INPUT;
InitRandom(ctrl->seed); InitRandom(ctrl->seed);
graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL); graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL);
AllocateWorkSpace(ctrl, graph); AllocateWorkSpace(ctrl, graph);
ncon = graph->ncon; ncon = graph->ncon;
ctrl->ncuts = 1; ctrl->ncuts = 1;
/* determine the weights of the two partitions as a function of the weight of the /* determine the weights of the two partitions as a function of the weight of the
target partition weights */ target partition weights */
tpwgts2 = rwspacemalloc(ctrl, 2*ncon); tpwgts2 = rwspacemalloc(ctrl, 2*ncon);
for (i=0; i<ncon; i++) { for (i=0; i<ncon; i++) {
tpwgts2[i] = rsum((2>>1), ctrl->tpwgts+i, ncon); tpwgts2[i] = rsum((2>>1), ctrl->tpwgts+i, ncon);
tpwgts2[ncon+i] = 1.0 - tpwgts2[i]; tpwgts2[ncon+i] = 1.0 - tpwgts2[i];
} }
/* perform the bisection */ /* perform the bisection */
*edgecut = MultilevelBisect(ctrl, graph, tpwgts2); *edgecut = MultilevelBisect(ctrl, graph, tpwgts2);
// ConstructMinCoverSeparator(&ctrl, &graph, 1.05); // ConstructMinCoverSeparator(&ctrl, &graph, 1.05);
// *edgecut = graph->mincut; // *edgecut = graph->mincut;
// *sepsize = graph.pwgts[2]; // *sepsize = graph.pwgts[2];
icopy(*nvtxs, graph->where, part); icopy(*nvtxs, graph->where, part);
std::cout << "Finished bisection:" << *edgecut << std::endl; std::cout << "Finished bisection:" << *edgecut << std::endl;
FreeGraph(&graph); FreeGraph(&graph);
FreeCtrl(&ctrl); FreeCtrl(&ctrl);
} }
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Return the number of edge cuts and the partition indices {part} * Return the number of edge cuts and the partition indices {part}
* Part [j] is 0 or 1, depending on * Part [j] is 0 or 1, depending on
* whether node j is in the left part of the graph or the right part respectively * whether node j is in the left part of the graph or the right part respectively
*/ */
std::pair<int, sharedInts> edgeMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy, std::pair<int, sharedInts> edgeMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy,
const sharedInts& adjwgt, bool verbose) { const sharedInts& adjwgt, bool verbose) {
// control parameters // control parameters
idx_t vwgt[n]; // the weights of the vertices idx_t vwgt[n]; // the weights of the vertices
idx_t options[METIS_NOPTIONS]; idx_t options[METIS_NOPTIONS];
METIS_SetDefaultOptions(options); // use defaults METIS_SetDefaultOptions(options); // use defaults
idx_t edgecut; // the number of edge cuts, output idx_t edgecut; // the number of edge cuts, output
sharedInts part_(new idx_t[n]); // the partition of each vertex, output sharedInts part_(new idx_t[n]); // the partition of each vertex, output
// set uniform weights on the vertices // set uniform weights on the vertices
std::fill(vwgt, vwgt+n, 1); std::fill(vwgt, vwgt+n, 1);
//TODO: Fix later //TODO: Fix later
//boost::timer TOTALTmr; //boost::timer TOTALTmr;
if (verbose) { if (verbose) {
printf("**********************************************************************\n"); printf("**********************************************************************\n");
printf("Graph Information ---------------------------------------------------\n"); printf("Graph Information ---------------------------------------------------\n");
printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2); printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2);
printf("\nND Partitioning... -------------------------------------------\n"); printf("\nND Partitioning... -------------------------------------------\n");
//cleartimer(TOTALTmr); //cleartimer(TOTALTmr);
//starttimer(TOTALTmr); //starttimer(TOTALTmr);
} }
//int wgtflag = 1; // only edge weights //int wgtflag = 1; // only edge weights
//int numflag = 0; // c style numbering starting from 0 //int numflag = 0; // c style numbering starting from 0
//int nparts = 2; // partition the graph to 2 submaps //int nparts = 2; // partition the graph to 2 submaps
modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(), modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(),
options, &edgecut, part_.get()); options, &edgecut, part_.get());
if (verbose) { if (verbose) {
//stoptimer(TOTALTmr); //stoptimer(TOTALTmr);
printf("\nTiming Information --------------------------------------------------\n"); printf("\nTiming Information --------------------------------------------------\n");
//printf(" Total: \t\t %7.3f\n", gettimer(TOTALTmr)); //printf(" Total: \t\t %7.3f\n", gettimer(TOTALTmr));
printf(" Edge cuts: \t\t %d\n", edgecut); printf(" Edge cuts: \t\t %d\n", edgecut);
printf("**********************************************************************\n"); printf("**********************************************************************\n");
} }
return std::make_pair(edgecut, part_); return std::make_pair(edgecut, part_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Prepare the data structure {xadj} and {adjncy} required by metis * Prepare the data structure {xadj} and {adjncy} required by metis
* xadj always has the size equal to the no. of the nodes plus 1 * xadj always has the size equal to the no. of the nodes plus 1
* adjncy always has the size equal to two times of the no. of the edges in the Metis graph * adjncy always has the size equal to two times of the no. of the edges in the Metis graph
*/ */
template <class GenericGraph> template <class GenericGraph>
void prepareMetisGraph(const GenericGraph& graph, const std::vector<size_t>& keys, WorkSpace& workspace, void prepareMetisGraph(const GenericGraph& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) { sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) {
typedef int Weight; typedef int Weight;
typedef std::vector<int> Weights; typedef std::vector<int> Weights;
typedef std::vector<int> Neighbors; typedef std::vector<int> Neighbors;
typedef std::pair<Neighbors, Weights> NeighborsInfo; typedef std::pair<Neighbors, Weights> NeighborsInfo;
// set up dictionary // set up dictionary
std::vector<int>& dictionary = workspace.dictionary; std::vector<int>& dictionary = workspace.dictionary;
workspace.prepareDictionary(keys); workspace.prepareDictionary(keys);
// prepare for {adjacencyMap}, a pair of neighbor indices and the correponding edge weights // prepare for {adjacencyMap}, a pair of neighbor indices and the correponding edge weights
int numNodes = keys.size(); int numNodes = keys.size();
int numEdges = 0; int numEdges = 0;
std::vector<NeighborsInfo> adjacencyMap; std::vector<NeighborsInfo> adjacencyMap;
adjacencyMap.resize(numNodes); adjacencyMap.resize(numNodes);
std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl; std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl;
int index1, index2; int index1, index2;
BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){
index1 = dictionary[factor->key1.index]; index1 = dictionary[factor->key1.index];
index2 = dictionary[factor->key2.index]; index2 = dictionary[factor->key2.index];
std::cout << "index1: " << index1 << std::endl; std::cout << "index1: " << index1 << std::endl;
std::cout << "index2: " << index2 << std::endl; std::cout << "index2: " << index2 << std::endl;
// if both nodes are in the current graph, i.e. not a joint factor between frontal and separator // if both nodes are in the current graph, i.e. not a joint factor between frontal and separator
if (index1 >= 0 && index2 >= 0) { if (index1 >= 0 && index2 >= 0) {
std::pair<Neighbors, Weights>& adjacencyMap1 = adjacencyMap[index1]; std::pair<Neighbors, Weights>& adjacencyMap1 = adjacencyMap[index1];
std::pair<Neighbors, Weights>& adjacencyMap2 = adjacencyMap[index2]; std::pair<Neighbors, Weights>& adjacencyMap2 = adjacencyMap[index2];
try{ try{
adjacencyMap1.first.push_back(index2); adjacencyMap1.first.push_back(index2);
adjacencyMap1.second.push_back(factor->weight); adjacencyMap1.second.push_back(factor->weight);
adjacencyMap2.first.push_back(index1); adjacencyMap2.first.push_back(index1);
adjacencyMap2.second.push_back(factor->weight); adjacencyMap2.second.push_back(factor->weight);
}catch(std::exception& e){ }catch(std::exception& e){
std::cout << e.what() << std::endl; std::cout << e.what() << std::endl;
} }
numEdges++; numEdges++;
} }
} }
// prepare for {xadj}, {adjncy}, and {adjwgt} // prepare for {xadj}, {adjncy}, and {adjwgt}
*ptr_xadj = sharedInts(new idx_t[numNodes+1]); *ptr_xadj = sharedInts(new idx_t[numNodes+1]);
*ptr_adjncy = sharedInts(new idx_t[numEdges*2]); *ptr_adjncy = sharedInts(new idx_t[numEdges*2]);
*ptr_adjwgt = sharedInts(new idx_t[numEdges*2]); *ptr_adjwgt = sharedInts(new idx_t[numEdges*2]);
sharedInts& xadj = *ptr_xadj; sharedInts& xadj = *ptr_xadj;
sharedInts& adjncy = *ptr_adjncy; sharedInts& adjncy = *ptr_adjncy;
sharedInts& adjwgt = *ptr_adjwgt; sharedInts& adjwgt = *ptr_adjwgt;
int ind_xadj = 0, ind_adjncy = 0; int ind_xadj = 0, ind_adjncy = 0;
BOOST_FOREACH(const NeighborsInfo& info, adjacencyMap) { BOOST_FOREACH(const NeighborsInfo& info, adjacencyMap) {
*(xadj.get() + ind_xadj) = ind_adjncy; *(xadj.get() + ind_xadj) = ind_adjncy;
std::copy(info.first .begin(), info.first .end(), adjncy.get() + ind_adjncy); std::copy(info.first .begin(), info.first .end(), adjncy.get() + ind_adjncy);
std::copy(info.second.begin(), info.second.end(), adjwgt.get() + ind_adjncy); std::copy(info.second.begin(), info.second.end(), adjwgt.get() + ind_adjncy);
assert(info.first.size() == info.second.size()); assert(info.first.size() == info.second.size());
ind_adjncy += info.first.size(); ind_adjncy += info.first.size();
ind_xadj ++; ind_xadj ++;
} }
if (ind_xadj != numNodes) throw std::runtime_error("prepareMetisGraph_: ind_xadj != numNodes"); if (ind_xadj != numNodes) throw std::runtime_error("prepareMetisGraph_: ind_xadj != numNodes");
*(xadj.get() + ind_xadj) = ind_adjncy; *(xadj.get() + ind_xadj) = ind_adjncy;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class GenericGraph> template<class GenericGraph>
boost::optional<MetisResult> separatorPartitionByMetis(const GenericGraph& graph, boost::optional<MetisResult> separatorPartitionByMetis(const GenericGraph& graph,
const std::vector<size_t>& keys, WorkSpace& workspace, bool verbose) { const std::vector<size_t>& keys, WorkSpace& workspace, bool verbose) {
// create a metis graph // create a metis graph
size_t numKeys = keys.size(); size_t numKeys = keys.size();
if (verbose) if (verbose)
std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl;
sharedInts xadj, adjncy, adjwgt; sharedInts xadj, adjncy, adjwgt;
prepareMetisGraph<GenericGraph>(graph, keys, workspace, &xadj, &adjncy, &adjwgt); prepareMetisGraph<GenericGraph>(graph, keys, workspace, &xadj, &adjncy, &adjwgt);
// run ND on the graph // run ND on the graph
size_t sepsize; size_t sepsize;
sharedInts part; sharedInts part;
boost::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose); boost::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose);
if (!sepsize) return boost::optional<MetisResult>(); if (!sepsize) return boost::optional<MetisResult>();
// convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later // convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later
// we will have more submaps // we will have more submaps
MetisResult result; MetisResult result;
result.C.reserve(sepsize); result.C.reserve(sepsize);
result.A.reserve(numKeys - sepsize); result.A.reserve(numKeys - sepsize);
result.B.reserve(numKeys - sepsize); result.B.reserve(numKeys - sepsize);
int* ptr_part = part.get(); int* ptr_part = part.get();
std::vector<size_t>::const_iterator itKey = keys.begin(); std::vector<size_t>::const_iterator itKey = keys.begin();
std::vector<size_t>::const_iterator itKeyLast = keys.end(); std::vector<size_t>::const_iterator itKeyLast = keys.end();
while(itKey != itKeyLast) { while(itKey != itKeyLast) {
switch(*(ptr_part++)) { switch(*(ptr_part++)) {
case 0: result.A.push_back(*(itKey++)); break; case 0: result.A.push_back(*(itKey++)); break;
case 1: result.B.push_back(*(itKey++)); break; case 1: result.B.push_back(*(itKey++)); break;
case 2: result.C.push_back(*(itKey++)); break; case 2: result.C.push_back(*(itKey++)); break;
default: throw std::runtime_error("separatorPartitionByMetis: invalid results from Metis ND!"); default: throw std::runtime_error("separatorPartitionByMetis: invalid results from Metis ND!");
} }
} }
if (verbose) { if (verbose) {
std::cout << "total key: " << keys.size() std::cout << "total key: " << keys.size()
<< " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", "
<< result.C.size() << "; sepsize from Metis = " << sepsize << std::endl; << result.C.size() << "; sepsize from Metis = " << sepsize << std::endl;
//throw runtime_error("separatorPartitionByMetis:stop for debug"); //throw runtime_error("separatorPartitionByMetis:stop for debug");
} }
if(result.C.size() != sepsize) { if(result.C.size() != sepsize) {
std::cout << "total key: " << keys.size() std::cout << "total key: " << keys.size()
<< " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size() << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size()
<< "; sepsize from Metis = " << sepsize << std::endl; << "; sepsize from Metis = " << sepsize << std::endl;
throw std::runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!"); throw std::runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!");
} }
return boost::make_optional<MetisResult >(result); return boost::make_optional<MetisResult >(result);
} }
/* *************************************************************************/ /* *************************************************************************/
template<class GenericGraph> template<class GenericGraph>
boost::optional<MetisResult> edgePartitionByMetis(const GenericGraph& graph, boost::optional<MetisResult> edgePartitionByMetis(const GenericGraph& graph,
const std::vector<size_t>& keys, WorkSpace& workspace, bool verbose) { const std::vector<size_t>& keys, WorkSpace& workspace, bool verbose) {
// a small hack for handling the camera1-camera2 case used in the unit tests // a small hack for handling the camera1-camera2 case used in the unit tests
if (graph.size() == 1 && keys.size() == 2) { if (graph.size() == 1 && keys.size() == 2) {
MetisResult result; MetisResult result;
result.A.push_back(keys.front()); result.A.push_back(keys.front());
result.B.push_back(keys.back()); result.B.push_back(keys.back());
return result; return result;
} }
// create a metis graph // create a metis graph
size_t numKeys = keys.size(); size_t numKeys = keys.size();
if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl;
sharedInts xadj, adjncy, adjwgt; sharedInts xadj, adjncy, adjwgt;
prepareMetisGraph<GenericGraph>(graph, keys, workspace, &xadj, &adjncy, &adjwgt); prepareMetisGraph<GenericGraph>(graph, keys, workspace, &xadj, &adjncy, &adjwgt);
// run metis on the graph // run metis on the graph
int edgecut; int edgecut;
sharedInts part; sharedInts part;
boost::tie(edgecut, part) = edgeMetis(numKeys, xadj, adjncy, adjwgt, verbose); boost::tie(edgecut, part) = edgeMetis(numKeys, xadj, adjncy, adjwgt, verbose);
// convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later we will have more submaps // convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later we will have more submaps
MetisResult result; MetisResult result;
result.A.reserve(numKeys); result.A.reserve(numKeys);
result.B.reserve(numKeys); result.B.reserve(numKeys);
int* ptr_part = part.get(); int* ptr_part = part.get();
std::vector<size_t>::const_iterator itKey = keys.begin(); std::vector<size_t>::const_iterator itKey = keys.begin();
std::vector<size_t>::const_iterator itKeyLast = keys.end(); std::vector<size_t>::const_iterator itKeyLast = keys.end();
while(itKey != itKeyLast) { while(itKey != itKeyLast) {
if (*ptr_part != 0 && *ptr_part != 1) if (*ptr_part != 0 && *ptr_part != 1)
std::cout << *ptr_part << "!!!" << std::endl; std::cout << *ptr_part << "!!!" << std::endl;
switch(*(ptr_part++)) { switch(*(ptr_part++)) {
case 0: result.A.push_back(*(itKey++)); break; case 0: result.A.push_back(*(itKey++)); break;
case 1: result.B.push_back(*(itKey++)); break; case 1: result.B.push_back(*(itKey++)); break;
default: throw std::runtime_error("edgePartitionByMetis: invalid results from Metis ND!"); default: throw std::runtime_error("edgePartitionByMetis: invalid results from Metis ND!");
} }
} }
if (verbose) { if (verbose) {
std::cout << "the size of two submaps in the reduced graph: " << result.A.size() std::cout << "the size of two submaps in the reduced graph: " << result.A.size()
<< " " << result.B.size() << std::endl; << " " << result.B.size() << std::endl;
int edgeCut = 0; int edgeCut = 0;
BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){
int key1 = factor->key1.index; int key1 = factor->key1.index;
int key2 = factor->key2.index; int key2 = factor->key2.index;
// print keys and their subgraph assignment // print keys and their subgraph assignment
std::cout << key1; std::cout << key1;
if (std::find(result.A.begin(), result.A.end(), key1) != result.A.end()) std::cout <<"A "; if (std::find(result.A.begin(), result.A.end(), key1) != result.A.end()) std::cout <<"A ";
if (std::find(result.B.begin(), result.B.end(), key1) != result.B.end()) std::cout <<"B "; if (std::find(result.B.begin(), result.B.end(), key1) != result.B.end()) std::cout <<"B ";
std::cout << key2; std::cout << key2;
if (std::find(result.A.begin(), result.A.end(), key2) != result.A.end()) std::cout <<"A "; if (std::find(result.A.begin(), result.A.end(), key2) != result.A.end()) std::cout <<"A ";
if (std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) std::cout <<"B "; if (std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) std::cout <<"B ";
std::cout << "weight " << factor->weight;; std::cout << "weight " << factor->weight;;
// find vertices that were assigned to sets A & B. Their edge will be cut // find vertices that were assigned to sets A & B. Their edge will be cut
if ((std::find(result.A.begin(), result.A.end(), key1) != result.A.end() && if ((std::find(result.A.begin(), result.A.end(), key1) != result.A.end() &&
std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) || std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) ||
(std::find(result.B.begin(), result.B.end(), key1) != result.B.end() && (std::find(result.B.begin(), result.B.end(), key1) != result.B.end() &&
std::find(result.A.begin(), result.A.end(), key2) != result.A.end())){ std::find(result.A.begin(), result.A.end(), key2) != result.A.end())){
edgeCut ++; edgeCut ++;
std::cout << " CUT "; std::cout << " CUT ";
} }
std::cout << std::endl; std::cout << std::endl;
} }
std::cout << "edgeCut: " << edgeCut << std::endl; std::cout << "edgeCut: " << edgeCut << std::endl;
} }
return boost::make_optional<MetisResult >(result); return boost::make_optional<MetisResult >(result);
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool isLargerIsland(const std::vector<size_t>& island1, const std::vector<size_t>& island2) { bool isLargerIsland(const std::vector<size_t>& island1, const std::vector<size_t>& island2) {
return island1.size() > island2.size(); return island1.size() > island2.size();
} }
/* ************************************************************************* */ /* ************************************************************************* */
// debug functions // debug functions
void printIsland(const std::vector<size_t>& island) { void printIsland(const std::vector<size_t>& island) {
std::cout << "island: "; std::cout << "island: ";
BOOST_FOREACH(const size_t key, island) BOOST_FOREACH(const size_t key, island)
std::cout << key << " "; std::cout << key << " ";
std::cout << std::endl; std::cout << std::endl;
} }
void printIslands(const std::list<std::vector<size_t> >& islands) { void printIslands(const std::list<std::vector<size_t> >& islands) {
BOOST_FOREACH(const std::vector<std::size_t>& island, islands) BOOST_FOREACH(const std::vector<std::size_t>& island, islands)
printIsland(island); printIsland(island);
} }
void printNumCamerasLandmarks(const std::vector<size_t>& keys, const std::vector<Symbol>& int2symbol) { void printNumCamerasLandmarks(const std::vector<size_t>& keys, const std::vector<Symbol>& int2symbol) {
int numCamera = 0, numLandmark = 0; int numCamera = 0, numLandmark = 0;
BOOST_FOREACH(const size_t key, keys) BOOST_FOREACH(const size_t key, keys)
if (int2symbol[key].chr() == 'x') if (int2symbol[key].chr() == 'x')
numCamera++; numCamera++;
else else
numLandmark++; numLandmark++;
std::cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << std::endl; std::cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << std::endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class GenericGraph> template<class GenericGraph>
void addLandmarkToPartitionResult(const GenericGraph& graph, const std::vector<size_t>& landmarkKeys, void addLandmarkToPartitionResult(const GenericGraph& graph, const std::vector<size_t>& landmarkKeys,
MetisResult& partitionResult, WorkSpace& workspace) { MetisResult& partitionResult, WorkSpace& workspace) {
// set up cameras in the dictionary // set up cameras in the dictionary
std::vector<size_t>& A = partitionResult.A; std::vector<size_t>& A = partitionResult.A;
std::vector<size_t>& B = partitionResult.B; std::vector<size_t>& B = partitionResult.B;
std::vector<size_t>& C = partitionResult.C; std::vector<size_t>& C = partitionResult.C;
std::vector<int>& dictionary = workspace.dictionary; std::vector<int>& dictionary = workspace.dictionary;
std::fill(dictionary.begin(), dictionary.end(), -1); std::fill(dictionary.begin(), dictionary.end(), -1);
BOOST_FOREACH(const size_t a, A) BOOST_FOREACH(const size_t a, A)
dictionary[a] = 1; dictionary[a] = 1;
BOOST_FOREACH(const size_t b, B) BOOST_FOREACH(const size_t b, B)
dictionary[b] = 2; dictionary[b] = 2;
if (!C.empty()) if (!C.empty())
throw std::runtime_error("addLandmarkToPartitionResult: C is not empty"); throw std::runtime_error("addLandmarkToPartitionResult: C is not empty");
// set up landmarks // set up landmarks
size_t i,j; size_t i,j;
BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) { BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) {
i = factor->key1.index; i = factor->key1.index;
j = factor->key2.index; j = factor->key2.index;
if (dictionary[j] == 0) // if the landmark is already in the separator, continue if (dictionary[j] == 0) // if the landmark is already in the separator, continue
continue; continue;
else if (dictionary[j] == -1) else if (dictionary[j] == -1)
dictionary[j] = dictionary[i]; dictionary[j] = dictionary[i];
else { else {
if (dictionary[j] != dictionary[i]) if (dictionary[j] != dictionary[i])
dictionary[j] = 0; dictionary[j] = 0;
} }
// if (j == 67980) // if (j == 67980)
// std::cout << "dictionary[67980]" << dictionary[j] << std::endl; // std::cout << "dictionary[67980]" << dictionary[j] << std::endl;
} }
BOOST_FOREACH(const size_t j, landmarkKeys) { BOOST_FOREACH(const size_t j, landmarkKeys) {
switch(dictionary[j]) { switch(dictionary[j]) {
case 0: C.push_back(j); break; case 0: C.push_back(j); break;
case 1: A.push_back(j); break; case 1: A.push_back(j); break;
case 2: B.push_back(j); break; case 2: B.push_back(j); break;
default: std::cout << j << ": " << dictionary[j] << std::endl; default: std::cout << j << ": " << dictionary[j] << std::endl;
throw std::runtime_error("addLandmarkToPartitionResult: wrong status for landmark"); throw std::runtime_error("addLandmarkToPartitionResult: wrong status for landmark");
} }
} }
} }
#define REDUCE_CAMERA_GRAPH #define REDUCE_CAMERA_GRAPH
/* ************************************************************************* */ /* ************************************************************************* */
template<class GenericGraph> template<class GenericGraph>
boost::optional<MetisResult> findPartitoning(const GenericGraph& graph, const std::vector<size_t>& keys, boost::optional<MetisResult> findPartitoning(const GenericGraph& graph, const std::vector<size_t>& keys,
WorkSpace& workspace, bool verbose, WorkSpace& workspace, bool verbose,
const boost::optional<std::vector<Symbol> >& int2symbol, const bool reduceGraph) { const boost::optional<std::vector<Symbol> >& int2symbol, const bool reduceGraph) {
boost::optional<MetisResult> result; boost::optional<MetisResult> result;
GenericGraph reducedGraph; GenericGraph reducedGraph;
std::vector<size_t> keyToPartition; std::vector<size_t> keyToPartition;
std::vector<size_t> cameraKeys, landmarkKeys; std::vector<size_t> cameraKeys, landmarkKeys;
if (reduceGraph) { if (reduceGraph) {
if (!int2symbol.is_initialized()) if (!int2symbol.is_initialized())
throw std::invalid_argument("findSeparator: int2symbol must be valid!"); throw std::invalid_argument("findSeparator: int2symbol must be valid!");
// find out all the landmark keys, which are to be eliminated // find out all the landmark keys, which are to be eliminated
cameraKeys.reserve(keys.size()); cameraKeys.reserve(keys.size());
landmarkKeys.reserve(keys.size()); landmarkKeys.reserve(keys.size());
BOOST_FOREACH(const size_t key, keys) { BOOST_FOREACH(const size_t key, keys) {
if((*int2symbol)[key].chr() == 'x') if((*int2symbol)[key].chr() == 'x')
cameraKeys.push_back(key); cameraKeys.push_back(key);
else else
landmarkKeys.push_back(key); landmarkKeys.push_back(key);
} }
keyToPartition = cameraKeys; keyToPartition = cameraKeys;
workspace.prepareDictionary(keyToPartition); workspace.prepareDictionary(keyToPartition);
const std::vector<int>& dictionary = workspace.dictionary; const std::vector<int>& dictionary = workspace.dictionary;
reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph); reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph);
std::cout << "original graph: V" << keys.size() << ", E" << graph.size() std::cout << "original graph: V" << keys.size() << ", E" << graph.size()
<< " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << std::endl; << " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << std::endl;
result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose); result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose);
} else // call Metis to partition the graph to A, B, C } else // call Metis to partition the graph to A, B, C
result = separatorPartitionByMetis(graph, keys, workspace, verbose); result = separatorPartitionByMetis(graph, keys, workspace, verbose);
if (!result.is_initialized()) { if (!result.is_initialized()) {
std::cout << "metis failed!" << std::endl; std::cout << "metis failed!" << std::endl;
return 0; return 0;
} }
if (reduceGraph) { if (reduceGraph) {
addLandmarkToPartitionResult(graph, landmarkKeys, *result, workspace); addLandmarkToPartitionResult(graph, landmarkKeys, *result, workspace);
std::cout << "the separator size: " << result->C.size() << " landmarks" << std::endl; std::cout << "the separator size: " << result->C.size() << " landmarks" << std::endl;
} }
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class GenericGraph> template<class GenericGraph>
int findSeparator(const GenericGraph& graph, const std::vector<size_t>& keys, int findSeparator(const GenericGraph& graph, const std::vector<size_t>& keys,
const int minNodesPerMap, WorkSpace& workspace, bool verbose, const int minNodesPerMap, WorkSpace& workspace, bool verbose,
const boost::optional<std::vector<Symbol> >& int2symbol, const bool reduceGraph, const boost::optional<std::vector<Symbol> >& int2symbol, const bool reduceGraph,
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
boost::optional<MetisResult> result = findPartitoning(graph, keys, workspace, boost::optional<MetisResult> result = findPartitoning(graph, keys, workspace,
verbose, int2symbol, reduceGraph); verbose, int2symbol, reduceGraph);
// find the island in A and B, and make them separated submaps // find the island in A and B, and make them separated submaps
typedef std::vector<size_t> Island; typedef std::vector<size_t> Island;
std::list<Island> islands; std::list<Island> islands;
std::list<Island> islands_in_A = findIslands(graph, result->A, workspace, std::list<Island> islands_in_A = findIslands(graph, result->A, workspace,
minNrConstraintsPerCamera, minNrConstraintsPerLandmark); minNrConstraintsPerCamera, minNrConstraintsPerLandmark);
std::list<Island> islands_in_B = findIslands(graph, result->B, workspace, std::list<Island> islands_in_B = findIslands(graph, result->B, workspace,
minNrConstraintsPerCamera, minNrConstraintsPerLandmark); minNrConstraintsPerCamera, minNrConstraintsPerLandmark);
islands.insert(islands.end(), islands_in_A.begin(), islands_in_A.end()); islands.insert(islands.end(), islands_in_A.begin(), islands_in_A.end());
islands.insert(islands.end(), islands_in_B.begin(), islands_in_B.end()); islands.insert(islands.end(), islands_in_B.begin(), islands_in_B.end());
islands.sort(isLargerIsland); islands.sort(isLargerIsland);
size_t numIsland0 = islands.size(); size_t numIsland0 = islands.size();
#ifdef NDEBUG #ifdef NDEBUG
// verbose = true; // verbose = true;
// if (!int2symbol) throw std::invalid_argument("findSeparator: int2symbol is not set!"); // if (!int2symbol) throw std::invalid_argument("findSeparator: int2symbol is not set!");
// std::cout << "sep size: " << result->C.size() << "; "; // std::cout << "sep size: " << result->C.size() << "; ";
// printNumCamerasLandmarks(result->C, *int2symbol); // printNumCamerasLandmarks(result->C, *int2symbol);
// std::cout << "no. of island: " << islands.size() << "; "; // std::cout << "no. of island: " << islands.size() << "; ";
// std::cout << "island size: "; // std::cout << "island size: ";
// BOOST_FOREACH(const Island& island, islands) // BOOST_FOREACH(const Island& island, islands)
// std::cout << island.size() << " "; // std::cout << island.size() << " ";
// std::cout << std::endl; // std::cout << std::endl;
// BOOST_FOREACH(const Island& island, islands) { // BOOST_FOREACH(const Island& island, islands) {
// printNumCamerasLandmarks(island, int2symbol); // printNumCamerasLandmarks(island, int2symbol);
// } // }
#endif #endif
// absorb small components into the separator // absorb small components into the separator
size_t oldSize = islands.size(); size_t oldSize = islands.size();
while(true) { while(true) {
if (islands.size() < 2) { if (islands.size() < 2) {
std::cout << "numIsland: " << numIsland0 << std::endl; std::cout << "numIsland: " << numIsland0 << std::endl;
throw std::runtime_error("findSeparator: found fewer than 2 submaps!"); throw std::runtime_error("findSeparator: found fewer than 2 submaps!");
} }
std::list<Island>::reference island = islands.back(); std::list<Island>::reference island = islands.back();
if ((int)island.size() >= minNodesPerMap) break; if ((int)island.size() >= minNodesPerMap) break;
result->C.insert(result->C.end(), island.begin(), island.end()); result->C.insert(result->C.end(), island.begin(), island.end());
islands.pop_back(); islands.pop_back();
} }
if (islands.size() != oldSize){ if (islands.size() != oldSize){
if (verbose) std::cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << std::endl; if (verbose) std::cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << std::endl;
} }
else{ else{
if (verbose) std::cout << oldSize << " submap(s);\t" << std::endl; if (verbose) std::cout << oldSize << " submap(s);\t" << std::endl;
} }
// generate the node map // generate the node map
std::vector<int>& partitionTable = workspace.partitionTable; std::vector<int>& partitionTable = workspace.partitionTable;
std::fill(partitionTable.begin(), partitionTable.end(), -1); std::fill(partitionTable.begin(), partitionTable.end(), -1);
BOOST_FOREACH(const size_t key, result->C) BOOST_FOREACH(const size_t key, result->C)
partitionTable[key] = 0; partitionTable[key] = 0;
int idx = 0; int idx = 0;
BOOST_FOREACH(const Island& island, islands) { BOOST_FOREACH(const Island& island, islands) {
idx++; idx++;
BOOST_FOREACH(const size_t key, island) { BOOST_FOREACH(const size_t key, island) {
partitionTable[key] = idx; partitionTable[key] = idx;
} }
} }
return islands.size(); return islands.size();
} }
}} //namespace }} //namespace

42
gtsam_unstable/partition/FindSeparator.h
View File

@ -16,29 +16,29 @@
namespace gtsam { namespace partition { namespace gtsam { namespace partition {
// typedef std::map<size_t, size_t> PartitionTable; // from the key to the partition: 0 - separator, > 1: submap id // typedef std::map<size_t, size_t> PartitionTable; // from the key to the partition: 0 - separator, > 1: submap id
/** the metis Nest dissection result */ /** the metis Nest dissection result */
struct MetisResult { struct MetisResult {
std::vector<size_t> A, B; // frontals std::vector<size_t> A, B; // frontals
std::vector<size_t> C; // separator std::vector<size_t> C; // separator
}; };
/** /**
* use Metis library to partition, return the size of separator and the optional partition table * use Metis library to partition, return the size of separator and the optional partition table
* the size of dictionary mush be equal to the number of variables in the original graph (the largest one) * the size of dictionary mush be equal to the number of variables in the original graph (the largest one)
*/ */
template<class GenericGraph> template<class GenericGraph>
boost::optional<MetisResult> separatorPartitionByMetis(const GenericGraph& graph, const std::vector<size_t>& keys, boost::optional<MetisResult> separatorPartitionByMetis(const GenericGraph& graph, const std::vector<size_t>& keys,
WorkSpace& workspace, bool verbose); WorkSpace& workspace, bool verbose);
/** /**
* return the number of submaps and the parition table of the partitioned graph (**stored in workspace.partitionTable**). * return the number of submaps and the parition table of the partitioned graph (**stored in workspace.partitionTable**).
* return 0 if failed Note that the original output of Metis is 0,1 for submap, and 2 for the separator. * return 0 if failed Note that the original output of Metis is 0,1 for submap, and 2 for the separator.
*/ */
template<class GenericGraph> template<class GenericGraph>
int findSeparator(const GenericGraph& graph, const std::vector<size_t>& keys, int findSeparator(const GenericGraph& graph, const std::vector<size_t>& keys,
const int minNodesPerMap, WorkSpace& workspace, bool verbose, const boost::optional<std::vector<Symbol> >& int2symbol, const int minNodesPerMap, WorkSpace& workspace, bool verbose, const boost::optional<std::vector<Symbol> >& int2symbol,
const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
}} //namespace }} //namespace

70
gtsam_unstable/partition/tests/testFindSeparator.cpp
View File

@ -185,45 +185,45 @@ TEST ( Partition, findSeparator2 )
// x25 x26 x27 x28 // x25 x26 x27 x28
TEST ( Partition, findSeparator3_with_reduced_camera ) TEST ( Partition, findSeparator3_with_reduced_camera )
{ {
GenericGraph3D graph; GenericGraph3D graph;
for (int j=1; j<=8; j++) for (int j=1; j<=8; j++)
graph.push_back(boost::make_shared<GenericFactor3D>(25, j)); graph.push_back(boost::make_shared<GenericFactor3D>(25, j));
for (int j=1; j<=16; j++) for (int j=1; j<=16; j++)
graph.push_back(boost::make_shared<GenericFactor3D>(26, j)); graph.push_back(boost::make_shared<GenericFactor3D>(26, j));
for (int j=9; j<=24; j++) for (int j=9; j<=24; j++)
graph.push_back(boost::make_shared<GenericFactor3D>(27, j)); graph.push_back(boost::make_shared<GenericFactor3D>(27, j));
for (int j=17; j<=24; j++) for (int j=17; j<=24; j++)
graph.push_back(boost::make_shared<GenericFactor3D>(28, j)); graph.push_back(boost::make_shared<GenericFactor3D>(28, j));
std::vector<size_t> keys; std::vector<size_t> keys;
for(int i=1; i<=28; i++) for(int i=1; i<=28; i++)
keys.push_back(i); keys.push_back(i);
vector<Symbol> int2symbol; vector<Symbol> int2symbol;
int2symbol.push_back(Symbol('x',0)); // dummy int2symbol.push_back(Symbol('x',0)); // dummy
for(int i=1; i<=24; i++) for(int i=1; i<=24; i++)
int2symbol.push_back(Symbol('l',i)); int2symbol.push_back(Symbol('l',i));
int2symbol.push_back(Symbol('x',25)); int2symbol.push_back(Symbol('x',25));
int2symbol.push_back(Symbol('x',26)); int2symbol.push_back(Symbol('x',26));
int2symbol.push_back(Symbol('x',27)); int2symbol.push_back(Symbol('x',27));
int2symbol.push_back(Symbol('x',28)); int2symbol.push_back(Symbol('x',28));
WorkSpace workspace(29); WorkSpace workspace(29);
bool reduceGraph = true; bool reduceGraph = true;
int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph, 0, 0); int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph, 0, 0);
LONGS_EQUAL(2, numIsland); LONGS_EQUAL(2, numIsland);
partition::PartitionTable& partitionTable = workspace.partitionTable; partition::PartitionTable& partitionTable = workspace.partitionTable;
for (int j=1; j<=8; j++) for (int j=1; j<=8; j++)
LONGS_EQUAL(1, partitionTable[j]); LONGS_EQUAL(1, partitionTable[j]);
for (int j=9; j<=16; j++) for (int j=9; j<=16; j++)
LONGS_EQUAL(0, partitionTable[j]); LONGS_EQUAL(0, partitionTable[j]);
for (int j=17; j<=24; j++) for (int j=17; j<=24; j++)
LONGS_EQUAL(2, partitionTable[j]); LONGS_EQUAL(2, partitionTable[j]);
LONGS_EQUAL(1, partitionTable[25]); LONGS_EQUAL(1, partitionTable[25]);
LONGS_EQUAL(1, partitionTable[26]); LONGS_EQUAL(1, partitionTable[26]);
LONGS_EQUAL(2, partitionTable[27]); LONGS_EQUAL(2, partitionTable[27]);
LONGS_EQUAL(2, partitionTable[28]); LONGS_EQUAL(2, partitionTable[28]);
} }
/* ************************************************************************* */ /* ************************************************************************* */

1
gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
View File

@ -121,3 +121,4 @@ int main()
TestResult tr; return TestRegistry::runAllTests(tr); TestResult tr; return TestRegistry::runAllTests(tr);
} }
/* ************************************************************************* */ /* ************************************************************************* */

49
tests/testGaussianFactorGraphB.cpp
View File

@ -593,55 +593,6 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) {
} }
} }
///* ************************************************************************* */
//TEST( GaussianFactorGraph, buildDualGraph1 )
//{
// GaussianFactorGraph fgc1 = createSimpleConstraintGraph();
// KeySet constrainedVariables1 = list_of(0)(1);
// VariableIndex variableIndex1(fgc1);
// VectorValues delta1 = createSimpleConstraintValues();
// GaussianFactorGraph::shared_ptr dualGraph1 = fgc1.buildDualGraph(
// constrainedVariables1, variableIndex1, delta1);
// GaussianFactorGraph expectedDualGraph1;
// expectedDualGraph1.push_back(JacobianFactor(3, eye(2), zero(2)));
// expectedDualGraph1.push_back(JacobianFactor(3, -eye(2), zero(2)));
// EXPECT(assert_equal(expectedDualGraph1, *dualGraph1));
//}
//
///* ************************************************************************* */
//TEST( GaussianFactorGraph, buildDualGraph2 )
//{
// GaussianFactorGraph fgc2 = createSingleConstraintGraph();
// KeySet constrainedVariables2 = list_of(0)(1);
// VariableIndex variableIndex2(fgc2);
// VectorValues delta2 = createSingleConstraintValues();
// GaussianFactorGraph::shared_ptr dualGraph2 = fgc2.buildDualGraph(
// constrainedVariables2, variableIndex2, delta2);
// GaussianFactorGraph expectedDualGraph2;
// expectedDualGraph2.push_back(JacobianFactor(3, (Matrix(2,2) << 1,2,2,1), zero(2)));
// expectedDualGraph2.push_back(JacobianFactor(3, 10*eye(2), zero(2)));
// EXPECT(assert_equal(expectedDualGraph2, *dualGraph2));
//}
//
///* ************************************************************************* */
//TEST( GaussianFactorGraph, buildDualGraph3 )
//{
// GaussianFactorGraph fgc3 = createMultiConstraintGraph();
// KeySet constrainedVariables3 = list_of(0)(1)(2);
// VariableIndex variableIndex3(fgc3);
// VectorValues delta3 = createMultiConstraintValues();
// GaussianFactorGraph::shared_ptr dualGraph3 = fgc3.buildDualGraph(
// constrainedVariables3, variableIndex3, delta3);
// GaussianFactorGraph expectedDualGraph3;
// expectedDualGraph3.push_back(
// JacobianFactor(3, (Matrix(2, 2) << 1, 2, 2, 1), 4,
// (Matrix(2, 2) << 3, -1, 4, -2), zero(2)));
// expectedDualGraph3.push_back(JacobianFactor(3, 10*eye(2), zero(2)));
// expectedDualGraph3.push_back(
// JacobianFactor(4, (Matrix(2, 2) << 1, 1, 1, 2), zero(2)));
// EXPECT(assert_equal(expectedDualGraph3, *dualGraph3));
//}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */ /* ************************************************************************* */

4
tests/testNonlinearOptimizer.cpp
View File

@ -273,11 +273,11 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
// test convergence // test convergence
Values actual = optimizer.optimize(); Values actual = optimizer.optimize();
EXPECT(assert_equal(expected, actual, 1e-6)); EXPECT(assert_equal(expected, actual));
// Check that the gradient is zero // Check that the gradient is zero
GaussianFactorGraph::shared_ptr linear = optimizer.linearize(); GaussianFactorGraph::shared_ptr linear = optimizer.linearize();
EXPECT(assert_equal(expectedGradient,linear->gradientAtZero(), 1e-7)); EXPECT(assert_equal(expectedGradient,linear->gradientAtZero()));
} }
EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize())); EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize()));