diff --git a/.gitignore b/.gitignore index d46bddd10..f3f1efd5b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ /build* +/doc* *.pyc *.DS_Store /examples/Data/dubrovnik-3-7-pre-rewritten.txt diff --git a/cmake/Config.cmake.in b/cmake/Config.cmake.in index 0ea8850a9..5f3956f07 100644 --- a/cmake/Config.cmake.in +++ b/cmake/Config.cmake.in @@ -6,7 +6,7 @@ get_filename_component(OUR_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) if(EXISTS "${OUR_CMAKE_DIR}/CMakeCache.txt") # 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() # Find installed library set(@PACKAGE_NAME@_INCLUDE_DIR "${OUR_CMAKE_DIR}/@CONF_REL_INCLUDE_DIR@" CACHE PATH "@PACKAGE_NAME@ include directory") diff --git a/gtsam.h b/gtsam.h index 51caf71be..cd540e39d 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1250,14 +1250,6 @@ virtual class JacobianFactor : gtsam::GaussianFactor { const gtsam::noiseModel::Diagonal* model); JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, 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); //Testable @@ -1664,7 +1656,6 @@ class NonlinearFactorGraph { gtsam::Ordering orderingCOLAMD() const; // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) 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; // enabling serialization functionality @@ -1675,7 +1666,6 @@ virtual class NonlinearFactor { // Factor base class size_t size() const; gtsam::KeyVector keys() const; - size_t dualKey() const; void print(string s) const; void printKeys(string s) const; // NonlinearFactor diff --git a/gtsam/3rdparty/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt index 80632e346..fd9c7eaf7 100644 --- a/gtsam/3rdparty/metis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/CMakeLists.txt @@ -4,11 +4,20 @@ project(METIS) # Add flags for currect directory and below if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") 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() -add_definitions(-Wno-unknown-pragmas) -#add_definitions(-Wunused-but-set-variable) +if(NOT ("${CMAKE_C_COMPILER_ID}" MATCHES "MSVC" OR "${CMAKE_CXX_COMPILER_ID}" MATCHES "MSVC")) + #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(SHARED FALSE CACHE BOOL "build a shared library") diff --git a/gtsam/3rdparty/metis/GKlib/gk_arch.h b/gtsam/3rdparty/metis/GKlib/gk_arch.h index 2cb80ccf2..5a21274f3 100644 --- a/gtsam/3rdparty/metis/GKlib/gk_arch.h +++ b/gtsam/3rdparty/metis/GKlib/gk_arch.h @@ -59,9 +59,10 @@ typedef ptrdiff_t ssize_t; #endif #ifdef __MSC__ +#if(_MSC_VER < 1700) /* MSC does not have rint() function */ #define rint(x) ((int)((x)+0.5)) - +#endif /* MSC does not have INFINITY defined */ #ifndef INFINITY #define INFINITY FLT_MAX diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index b49411b12..1469e265d 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -543,8 +543,7 @@ Matrix collect(size_t nrMatrices, ...) void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask) { const DenseIndex m = A.rows(); if (inf_mask) { - // only scale the first v.size() rows of A to support augmented Matrix - for (DenseIndex i=0; i - 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)) return true; printf("Not equal:\n"); diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 8c4ebb963..a9ef8fe10 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -281,17 +281,12 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights, size_t m = weights.size(); static const double inf = std::numeric_limits::infinity(); - // Check once for zero entries of a. + // Check once for zero entries of a. TODO: really needed ? vector isZero; - for (size_t i = 0; i < m; ++i) { - 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; - } + for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9); + // If there is a valid (a!=0) constraint (sigma==0) return the first one 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]) { // Basically, instead of doing a normal QR step with the weighted // pseudoinverse, we enforce the constraint by turning diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 455e8501d..674cc7bf8 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -300,7 +300,7 @@ namespace gtsam { } /* ************************************************************************* */ -VectorValues GaussianFactorGraph::gradientAtZero() const { + VectorValues GaussianFactorGraph::gradientAtZero() const { // Zero-out the gradient VectorValues g; BOOST_FOREACH(const sharedFactor& factor, *this) { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 3cc2d084c..910b25d1e 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -262,6 +262,7 @@ namespace gtsam { /** * 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$. + * @param fg The Jacobian factor graph $(A,b)$ * @param [output] g A VectorValues to store the gradient, which must be preallocated, * see allocateVectorValues * @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 graphs - */ - boost::tuple splitConstraints() const; - private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 0f6ba2d3a..167557162 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -650,7 +650,6 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) HessianFactor::shared_ptr jointFactor; try { jointFactor = boost::make_shared(factors, Scatter(factors, keys)); -// jointFactor->print("jointFactor: "); } catch(std::invalid_argument&) { throw InvalidDenseElimination( "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 jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate); } catch(CholeskyFailed&) { -// std::cout << "Problematic Hessian: " << jointFactor->information() << std::endl; throw IndeterminantLinearSystemException(keys.front()); } diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 679851d2d..8af5277e2 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -384,10 +384,7 @@ namespace gtsam { void multiplyHessianAdd(double alpha, const double* x, double* y) const {}; - /** - * eta for Hessian - * Ignore duals parameters. It's only valid for constraints, which need to be a JacobianFactor - */ + /// eta for Hessian VectorValues gradientAtZero() const; virtual void gradientAtZero(double* d) const; diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index 5b9126975..6c4cb969a 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -19,98 +19,90 @@ #pragma once #include -#include -#include -#include -#include namespace gtsam { -/* ************************************************************************* */ -template -JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, - const SharedDiagonal& model) { - fillTerms(terms, b, model); -} - -/* ************************************************************************* */ -template -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& p) { - return p.second.cols(); -} -} - -/* ************************************************************************* */ -template -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& 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; + /* ************************************************************************* */ + template + JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, const SharedDiagonal& model) + { + fillTerms(terms, b, model); } - // Assign RHS vector - getb() = b; + /* ************************************************************************* */ + template + 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 - model_ = noiseModel; -} + // 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; + } + + /* ************************************************************************* */ + template + 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 dimensions; + dimensions.reserve(terms.size()); + for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) { + const std::pair& 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& 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 diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index ab607875d..664fcf3b7 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -66,15 +66,6 @@ namespace gtsam { 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) { @@ -316,22 +307,6 @@ namespace gtsam { 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 { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index c13c45adb..290249b68 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -130,15 +130,12 @@ namespace gtsam { /** Create a VectorValues with the same structure as \c other, but filled with zeros. */ 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 /// @{ /** Number of variables stored. */ - size_t size() const { return values_.size(); } + Key size() const { return values_.size(); } /** Return the dimension of variable \c j. */ size_t dim(Key j) const { return at(j).rows(); } @@ -305,10 +302,6 @@ namespace gtsam { * structure (checked when NDEBUG is not defined). */ 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. */ friend GTSAM_EXPORT VectorValues operator*(const double a, const VectorValues &v); diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 835eaa645..7b2d59171 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -105,6 +105,24 @@ TEST(JacobianFactor, constructors_and_accessors) EXPECT(noise == expected.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 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 JacobianFactor expected( @@ -576,7 +594,7 @@ TEST ( JacobianFactor, constraint_eliminate2 ) Matrix m(1,2); Matrix Aempty = m.topRows(0); 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)); // verify CG diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 133219b9e..95e635516 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -48,15 +48,6 @@ class Dummy { unsigned char dummyTwoVar(unsigned char a) const; }; -#include -class QPSolver { - QPSolver(const gtsam::GaussianFactorGraph &graph); - pair optimize(const gtsam::VectorValues& initials) const; - pair optimize() const; - pair findFeasibleInitialValues() const; -}; - - #include class PoseRTV { PoseRTV(); diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index 5c33c94a1..ace13dc41 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -28,539 +28,539 @@ extern "C" { namespace gtsam { namespace partition { - typedef boost::shared_array sharedInts; + typedef boost::shared_array sharedInts; - /* ************************************************************************* */ - /** - * Return the size of the separator and the partiion indices {part} - * 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 - * separator, respectively - */ - std::pair separatorMetis(idx_t n, const sharedInts& xadj, - const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) { + /* ************************************************************************* */ + /** + * Return the size of the separator and the partiion indices {part} + * 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 + * separator, respectively + */ + std::pair separatorMetis(idx_t n, const sharedInts& xadj, + const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) { - // control parameters - idx_t vwgt[n]; // the weights of the vertices - idx_t options[METIS_NOPTIONS]; - METIS_SetDefaultOptions(options); // use defaults - idx_t sepsize; // the size of the separator, output - sharedInts part_(new idx_t[n]); // the partition of each vertex, output + // control parameters + idx_t vwgt[n]; // the weights of the vertices + idx_t options[METIS_NOPTIONS]; + METIS_SetDefaultOptions(options); // use defaults + idx_t sepsize; // the size of the separator, output + sharedInts part_(new idx_t[n]); // the partition of each vertex, output - // set uniform weights on the vertices - std::fill(vwgt, vwgt+n, 1); + // set uniform weights on the vertices + std::fill(vwgt, vwgt+n, 1); - // TODO: Fix at later time - //boost::timer::cpu_timer TOTALTmr; - if (verbose) { - printf("**********************************************************************\n"); - printf("Graph Information ---------------------------------------------------\n"); - printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2); - printf("\nND Partitioning... -------------------------------------------\n"); - //TOTALTmr.start() - } + // TODO: Fix at later time + //boost::timer::cpu_timer TOTALTmr; + if (verbose) { + printf("**********************************************************************\n"); + printf("Graph Information ---------------------------------------------------\n"); + printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2); + printf("\nND Partitioning... -------------------------------------------\n"); + //TOTALTmr.start() + } - // call metis parition routine - METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(), + // call metis parition routine + METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(), vwgt, options, &sepsize, part_.get()); - if (verbose) { - //boost::cpu_times const elapsed_times(timer.elapsed()); - //printf("\nTiming Information --------------------------------------------------\n"); - //printf(" Total: \t\t %7.3f\n", elapsed_times); - printf(" Sep size: \t\t %d\n", sepsize); - printf("**********************************************************************\n"); - } + if (verbose) { + //boost::cpu_times const elapsed_times(timer.elapsed()); + //printf("\nTiming Information --------------------------------------------------\n"); + //printf(" Total: \t\t %7.3f\n", elapsed_times); + printf(" Sep size: \t\t %d\n", sepsize); + 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, - idx_t *adjwgt, idx_t *options, idx_t *edgecut, idx_t *part) - { + /* ************************************************************************* */ + 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 i, ncon; - graph_t *graph; - real_t *tpwgts2; - ctrl_t *ctrl; - ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL); - ctrl->iptype = METIS_IPTYPE_GROW; - //if () == NULL) - // return METIS_ERROR_INPUT; + graph_t *graph; + real_t *tpwgts2; + ctrl_t *ctrl; + ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL); + ctrl->iptype = METIS_IPTYPE_GROW; + //if () == NULL) + // 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; - ctrl->ncuts = 1; - - /* determine the weights of the two partitions as a function of the weight of the - target partition weights */ + ncon = graph->ncon; + ctrl->ncuts = 1; + + /* determine the weights of the two partitions as a function of the weight of the + target partition weights */ - tpwgts2 = rwspacemalloc(ctrl, 2*ncon); - for (i=0; i>1), ctrl->tpwgts+i, ncon); - tpwgts2[ncon+i] = 1.0 - tpwgts2[i]; - } - /* perform the bisection */ - *edgecut = MultilevelBisect(ctrl, graph, tpwgts2); + tpwgts2 = rwspacemalloc(ctrl, 2*ncon); + for (i=0; i>1), ctrl->tpwgts+i, ncon); + tpwgts2[ncon+i] = 1.0 - tpwgts2[i]; + } + /* perform the bisection */ + *edgecut = MultilevelBisect(ctrl, graph, tpwgts2); - // ConstructMinCoverSeparator(&ctrl, &graph, 1.05); - // *edgecut = graph->mincut; - // *sepsize = graph.pwgts[2]; - icopy(*nvtxs, graph->where, part); - std::cout << "Finished bisection:" << *edgecut << std::endl; - FreeGraph(&graph); + // ConstructMinCoverSeparator(&ctrl, &graph, 1.05); + // *edgecut = graph->mincut; + // *sepsize = graph.pwgts[2]; + icopy(*nvtxs, graph->where, part); + std::cout << "Finished bisection:" << *edgecut << std::endl; + FreeGraph(&graph); - FreeCtrl(&ctrl); - } + FreeCtrl(&ctrl); + } - /* ************************************************************************* */ - /** - * Return the number of edge cuts and the partition indices {part} - * Part [j] is 0 or 1, depending on - * whether node j is in the left part of the graph or the right part respectively - */ - std::pair edgeMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy, - const sharedInts& adjwgt, bool verbose) { + /* ************************************************************************* */ + /** + * Return the number of edge cuts and the partition indices {part} + * Part [j] is 0 or 1, depending on + * whether node j is in the left part of the graph or the right part respectively + */ + std::pair edgeMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy, + const sharedInts& adjwgt, bool verbose) { - // control parameters - idx_t vwgt[n]; // the weights of the vertices - idx_t options[METIS_NOPTIONS]; - METIS_SetDefaultOptions(options); // use defaults - idx_t edgecut; // the number of edge cuts, output - sharedInts part_(new idx_t[n]); // the partition of each vertex, output + // control parameters + idx_t vwgt[n]; // the weights of the vertices + idx_t options[METIS_NOPTIONS]; + METIS_SetDefaultOptions(options); // use defaults + idx_t edgecut; // the number of edge cuts, output + sharedInts part_(new idx_t[n]); // the partition of each vertex, output - // set uniform weights on the vertices - std::fill(vwgt, vwgt+n, 1); + // set uniform weights on the vertices + std::fill(vwgt, vwgt+n, 1); - //TODO: Fix later - //boost::timer TOTALTmr; - if (verbose) { - printf("**********************************************************************\n"); - printf("Graph Information ---------------------------------------------------\n"); - printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2); - printf("\nND Partitioning... -------------------------------------------\n"); - //cleartimer(TOTALTmr); - //starttimer(TOTALTmr); - } + //TODO: Fix later + //boost::timer TOTALTmr; + if (verbose) { + printf("**********************************************************************\n"); + printf("Graph Information ---------------------------------------------------\n"); + printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2); + printf("\nND Partitioning... -------------------------------------------\n"); + //cleartimer(TOTALTmr); + //starttimer(TOTALTmr); + } - //int wgtflag = 1; // only edge weights - //int numflag = 0; // c style numbering starting from 0 - //int nparts = 2; // partition the graph to 2 submaps - modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(), - options, &edgecut, part_.get()); + //int wgtflag = 1; // only edge weights + //int numflag = 0; // c style numbering starting from 0 + //int nparts = 2; // partition the graph to 2 submaps + modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(), + options, &edgecut, part_.get()); - - if (verbose) { - //stoptimer(TOTALTmr); - printf("\nTiming Information --------------------------------------------------\n"); - //printf(" Total: \t\t %7.3f\n", gettimer(TOTALTmr)); - printf(" Edge cuts: \t\t %d\n", edgecut); - printf("**********************************************************************\n"); - } + + if (verbose) { + //stoptimer(TOTALTmr); + printf("\nTiming Information --------------------------------------------------\n"); + //printf(" Total: \t\t %7.3f\n", gettimer(TOTALTmr)); + printf(" Edge cuts: \t\t %d\n", edgecut); + printf("**********************************************************************\n"); + } - return std::make_pair(edgecut, part_); - } + return std::make_pair(edgecut, part_); + } - /* ************************************************************************* */ - /** - * Prepare the data structure {xadj} and {adjncy} required by metis - * 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 - */ - template - void prepareMetisGraph(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, - sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) { + /* ************************************************************************* */ + /** + * Prepare the data structure {xadj} and {adjncy} required by metis + * 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 + */ + template + void prepareMetisGraph(const GenericGraph& graph, const std::vector& keys, WorkSpace& workspace, + sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) { - typedef int Weight; - typedef std::vector Weights; - typedef std::vector Neighbors; - typedef std::pair NeighborsInfo; + typedef int Weight; + typedef std::vector Weights; + typedef std::vector Neighbors; + typedef std::pair NeighborsInfo; - // set up dictionary - std::vector& dictionary = workspace.dictionary; - workspace.prepareDictionary(keys); + // set up dictionary + std::vector& dictionary = workspace.dictionary; + workspace.prepareDictionary(keys); - // prepare for {adjacencyMap}, a pair of neighbor indices and the correponding edge weights - int numNodes = keys.size(); - int numEdges = 0; - std::vector adjacencyMap; - adjacencyMap.resize(numNodes); - std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl; - int index1, index2; + // prepare for {adjacencyMap}, a pair of neighbor indices and the correponding edge weights + int numNodes = keys.size(); + int numEdges = 0; + std::vector adjacencyMap; + adjacencyMap.resize(numNodes); + std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl; + int index1, index2; - BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ - index1 = dictionary[factor->key1.index]; - index2 = dictionary[factor->key2.index]; - std::cout << "index1: " << index1 << 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 (index1 >= 0 && index2 >= 0) { - std::pair& adjacencyMap1 = adjacencyMap[index1]; - std::pair& adjacencyMap2 = adjacencyMap[index2]; - try{ - adjacencyMap1.first.push_back(index2); - adjacencyMap1.second.push_back(factor->weight); - adjacencyMap2.first.push_back(index1); - adjacencyMap2.second.push_back(factor->weight); - }catch(std::exception& e){ - std::cout << e.what() << std::endl; - } - numEdges++; - } - } + BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ + index1 = dictionary[factor->key1.index]; + index2 = dictionary[factor->key2.index]; + std::cout << "index1: " << index1 << 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 (index1 >= 0 && index2 >= 0) { + std::pair& adjacencyMap1 = adjacencyMap[index1]; + std::pair& adjacencyMap2 = adjacencyMap[index2]; + try{ + adjacencyMap1.first.push_back(index2); + adjacencyMap1.second.push_back(factor->weight); + adjacencyMap2.first.push_back(index1); + adjacencyMap2.second.push_back(factor->weight); + }catch(std::exception& e){ + std::cout << e.what() << std::endl; + } + numEdges++; + } + } - // prepare for {xadj}, {adjncy}, and {adjwgt} - *ptr_xadj = sharedInts(new idx_t[numNodes+1]); - *ptr_adjncy = sharedInts(new idx_t[numEdges*2]); - *ptr_adjwgt = sharedInts(new idx_t[numEdges*2]); - sharedInts& xadj = *ptr_xadj; - sharedInts& adjncy = *ptr_adjncy; - sharedInts& adjwgt = *ptr_adjwgt; - int ind_xadj = 0, ind_adjncy = 0; - BOOST_FOREACH(const NeighborsInfo& info, adjacencyMap) { - *(xadj.get() + ind_xadj) = 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); - assert(info.first.size() == info.second.size()); - ind_adjncy += info.first.size(); - ind_xadj ++; - } - if (ind_xadj != numNodes) throw std::runtime_error("prepareMetisGraph_: ind_xadj != numNodes"); - *(xadj.get() + ind_xadj) = ind_adjncy; - } + // prepare for {xadj}, {adjncy}, and {adjwgt} + *ptr_xadj = sharedInts(new idx_t[numNodes+1]); + *ptr_adjncy = sharedInts(new idx_t[numEdges*2]); + *ptr_adjwgt = sharedInts(new idx_t[numEdges*2]); + sharedInts& xadj = *ptr_xadj; + sharedInts& adjncy = *ptr_adjncy; + sharedInts& adjwgt = *ptr_adjwgt; + int ind_xadj = 0, ind_adjncy = 0; + BOOST_FOREACH(const NeighborsInfo& info, adjacencyMap) { + *(xadj.get() + ind_xadj) = 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); + assert(info.first.size() == info.second.size()); + ind_adjncy += info.first.size(); + ind_xadj ++; + } + if (ind_xadj != numNodes) throw std::runtime_error("prepareMetisGraph_: ind_xadj != numNodes"); + *(xadj.get() + ind_xadj) = ind_adjncy; + } - /* ************************************************************************* */ - template - boost::optional separatorPartitionByMetis(const GenericGraph& graph, - const std::vector& keys, WorkSpace& workspace, bool verbose) { - // create a metis graph - size_t numKeys = keys.size(); - if (verbose) - std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; + /* ************************************************************************* */ + template + boost::optional separatorPartitionByMetis(const GenericGraph& graph, + const std::vector& keys, WorkSpace& workspace, bool verbose) { + // create a metis graph + size_t numKeys = keys.size(); + if (verbose) + std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; - sharedInts xadj, adjncy, adjwgt; + sharedInts xadj, adjncy, adjwgt; - prepareMetisGraph(graph, keys, workspace, &xadj, &adjncy, &adjwgt); + prepareMetisGraph(graph, keys, workspace, &xadj, &adjncy, &adjwgt); - // run ND on the graph - size_t sepsize; - sharedInts part; - boost::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose); - if (!sepsize) return boost::optional(); + // run ND on the graph + size_t sepsize; + sharedInts part; + boost::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose); + if (!sepsize) return boost::optional(); - // 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; - result.C.reserve(sepsize); - result.A.reserve(numKeys - sepsize); - result.B.reserve(numKeys - sepsize); - int* ptr_part = part.get(); - std::vector::const_iterator itKey = keys.begin(); - std::vector::const_iterator itKeyLast = keys.end(); - while(itKey != itKeyLast) { - switch(*(ptr_part++)) { - case 0: result.A.push_back(*(itKey++)); break; - case 1: result.B.push_back(*(itKey++)); break; - case 2: result.C.push_back(*(itKey++)); break; - default: throw std::runtime_error("separatorPartitionByMetis: invalid results from Metis ND!"); - } - } + // 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; + result.C.reserve(sepsize); + result.A.reserve(numKeys - sepsize); + result.B.reserve(numKeys - sepsize); + int* ptr_part = part.get(); + std::vector::const_iterator itKey = keys.begin(); + std::vector::const_iterator itKeyLast = keys.end(); + while(itKey != itKeyLast) { + switch(*(ptr_part++)) { + case 0: result.A.push_back(*(itKey++)); break; + case 1: result.B.push_back(*(itKey++)); break; + case 2: result.C.push_back(*(itKey++)); break; + default: throw std::runtime_error("separatorPartitionByMetis: invalid results from Metis ND!"); + } + } - if (verbose) { - std::cout << "total key: " << keys.size() - << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " - << result.C.size() << "; sepsize from Metis = " << sepsize << std::endl; - //throw runtime_error("separatorPartitionByMetis:stop for debug"); - } + if (verbose) { + std::cout << "total key: " << keys.size() + << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " + << result.C.size() << "; sepsize from Metis = " << sepsize << std::endl; + //throw runtime_error("separatorPartitionByMetis:stop for debug"); + } - if(result.C.size() != sepsize) { - std::cout << "total key: " << keys.size() - << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size() - << "; sepsize from Metis = " << sepsize << std::endl; - throw std::runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!"); - } + if(result.C.size() != sepsize) { + std::cout << "total key: " << keys.size() + << " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size() + << "; sepsize from Metis = " << sepsize << std::endl; + throw std::runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!"); + } - return boost::make_optional(result); - } + return boost::make_optional(result); + } - /* *************************************************************************/ - template - boost::optional edgePartitionByMetis(const GenericGraph& graph, - const std::vector& keys, WorkSpace& workspace, bool verbose) { + /* *************************************************************************/ + template + boost::optional edgePartitionByMetis(const GenericGraph& graph, + const std::vector& keys, WorkSpace& workspace, bool verbose) { - // a small hack for handling the camera1-camera2 case used in the unit tests - if (graph.size() == 1 && keys.size() == 2) { - MetisResult result; - result.A.push_back(keys.front()); - result.B.push_back(keys.back()); - return result; - } + // a small hack for handling the camera1-camera2 case used in the unit tests + if (graph.size() == 1 && keys.size() == 2) { + MetisResult result; + result.A.push_back(keys.front()); + result.B.push_back(keys.back()); + return result; + } - // create a metis graph - size_t numKeys = keys.size(); - if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; - sharedInts xadj, adjncy, adjwgt; - prepareMetisGraph(graph, keys, workspace, &xadj, &adjncy, &adjwgt); + // create a metis graph + size_t numKeys = keys.size(); + if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl; + sharedInts xadj, adjncy, adjwgt; + prepareMetisGraph(graph, keys, workspace, &xadj, &adjncy, &adjwgt); - // run metis on the graph - int edgecut; - sharedInts part; - boost::tie(edgecut, part) = edgeMetis(numKeys, xadj, adjncy, adjwgt, verbose); + // run metis on the graph + int edgecut; + sharedInts part; + 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 - MetisResult result; - result.A.reserve(numKeys); - result.B.reserve(numKeys); - int* ptr_part = part.get(); - std::vector::const_iterator itKey = keys.begin(); - std::vector::const_iterator itKeyLast = keys.end(); - while(itKey != itKeyLast) { - if (*ptr_part != 0 && *ptr_part != 1) - std::cout << *ptr_part << "!!!" << std::endl; - switch(*(ptr_part++)) { - case 0: result.A.push_back(*(itKey++)); break; - case 1: result.B.push_back(*(itKey++)); break; - default: throw std::runtime_error("edgePartitionByMetis: invalid results from Metis ND!"); - } - } + // 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; + result.A.reserve(numKeys); + result.B.reserve(numKeys); + int* ptr_part = part.get(); + std::vector::const_iterator itKey = keys.begin(); + std::vector::const_iterator itKeyLast = keys.end(); + while(itKey != itKeyLast) { + if (*ptr_part != 0 && *ptr_part != 1) + std::cout << *ptr_part << "!!!" << std::endl; + switch(*(ptr_part++)) { + case 0: result.A.push_back(*(itKey++)); break; + case 1: result.B.push_back(*(itKey++)); break; + default: throw std::runtime_error("edgePartitionByMetis: invalid results from Metis ND!"); + } + } - if (verbose) { - std::cout << "the size of two submaps in the reduced graph: " << result.A.size() - << " " << result.B.size() << std::endl; - int edgeCut = 0; + if (verbose) { + std::cout << "the size of two submaps in the reduced graph: " << result.A.size() + << " " << result.B.size() << std::endl; + int edgeCut = 0; - BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ - int key1 = factor->key1.index; - int key2 = factor->key2.index; - // print keys and their subgraph assignment - std::cout << key1; - 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 "; + BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){ + int key1 = factor->key1.index; + int key2 = factor->key2.index; + // print keys and their subgraph assignment + std::cout << key1; + 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 "; - std::cout << key2; - 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 "; + std::cout << key2; + 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 "; std::cout << "weight " << factor->weight;; - // 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() && - 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.A.begin(), result.A.end(), key2) != result.A.end())){ - edgeCut ++; - std::cout << " CUT "; - } - std::cout << std::endl; - } - std::cout << "edgeCut: " << edgeCut << std::endl; - } + // 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() && + 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.A.begin(), result.A.end(), key2) != result.A.end())){ + edgeCut ++; + std::cout << " CUT "; + } + std::cout << std::endl; + } + std::cout << "edgeCut: " << edgeCut << std::endl; + } - return boost::make_optional(result); - } + return boost::make_optional(result); + } - /* ************************************************************************* */ - bool isLargerIsland(const std::vector& island1, const std::vector& island2) { - return island1.size() > island2.size(); - } + /* ************************************************************************* */ + bool isLargerIsland(const std::vector& island1, const std::vector& island2) { + return island1.size() > island2.size(); + } - /* ************************************************************************* */ - // debug functions - void printIsland(const std::vector& island) { - std::cout << "island: "; - BOOST_FOREACH(const size_t key, island) - std::cout << key << " "; - std::cout << std::endl; - } + /* ************************************************************************* */ + // debug functions + void printIsland(const std::vector& island) { + std::cout << "island: "; + BOOST_FOREACH(const size_t key, island) + std::cout << key << " "; + std::cout << std::endl; + } - void printIslands(const std::list >& islands) { - BOOST_FOREACH(const std::vector& island, islands) - printIsland(island); - } + void printIslands(const std::list >& islands) { + BOOST_FOREACH(const std::vector& island, islands) + printIsland(island); + } - void printNumCamerasLandmarks(const std::vector& keys, const std::vector& int2symbol) { - int numCamera = 0, numLandmark = 0; - BOOST_FOREACH(const size_t key, keys) - if (int2symbol[key].chr() == 'x') - numCamera++; - else - numLandmark++; - std::cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << std::endl; - } + void printNumCamerasLandmarks(const std::vector& keys, const std::vector& int2symbol) { + int numCamera = 0, numLandmark = 0; + BOOST_FOREACH(const size_t key, keys) + if (int2symbol[key].chr() == 'x') + numCamera++; + else + numLandmark++; + std::cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << std::endl; + } - /* ************************************************************************* */ - template - void addLandmarkToPartitionResult(const GenericGraph& graph, const std::vector& landmarkKeys, - MetisResult& partitionResult, WorkSpace& workspace) { + /* ************************************************************************* */ + template + void addLandmarkToPartitionResult(const GenericGraph& graph, const std::vector& landmarkKeys, + MetisResult& partitionResult, WorkSpace& workspace) { - // set up cameras in the dictionary - std::vector& A = partitionResult.A; - std::vector& B = partitionResult.B; - std::vector& C = partitionResult.C; - std::vector& dictionary = workspace.dictionary; - std::fill(dictionary.begin(), dictionary.end(), -1); - BOOST_FOREACH(const size_t a, A) - dictionary[a] = 1; - BOOST_FOREACH(const size_t b, B) - dictionary[b] = 2; - if (!C.empty()) - throw std::runtime_error("addLandmarkToPartitionResult: C is not empty"); + // set up cameras in the dictionary + std::vector& A = partitionResult.A; + std::vector& B = partitionResult.B; + std::vector& C = partitionResult.C; + std::vector& dictionary = workspace.dictionary; + std::fill(dictionary.begin(), dictionary.end(), -1); + BOOST_FOREACH(const size_t a, A) + dictionary[a] = 1; + BOOST_FOREACH(const size_t b, B) + dictionary[b] = 2; + if (!C.empty()) + throw std::runtime_error("addLandmarkToPartitionResult: C is not empty"); - // set up landmarks - size_t i,j; - BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) { - i = factor->key1.index; - j = factor->key2.index; - if (dictionary[j] == 0) // if the landmark is already in the separator, continue - continue; - else if (dictionary[j] == -1) - dictionary[j] = dictionary[i]; - else { - if (dictionary[j] != dictionary[i]) - dictionary[j] = 0; - } -// if (j == 67980) -// std::cout << "dictionary[67980]" << dictionary[j] << std::endl; - } + // set up landmarks + size_t i,j; + BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) { + i = factor->key1.index; + j = factor->key2.index; + if (dictionary[j] == 0) // if the landmark is already in the separator, continue + continue; + else if (dictionary[j] == -1) + dictionary[j] = dictionary[i]; + else { + if (dictionary[j] != dictionary[i]) + dictionary[j] = 0; + } +// if (j == 67980) +// std::cout << "dictionary[67980]" << dictionary[j] << std::endl; + } - BOOST_FOREACH(const size_t j, landmarkKeys) { - switch(dictionary[j]) { - case 0: C.push_back(j); break; - case 1: A.push_back(j); break; - case 2: B.push_back(j); break; - default: std::cout << j << ": " << dictionary[j] << std::endl; - throw std::runtime_error("addLandmarkToPartitionResult: wrong status for landmark"); - } - } - } + BOOST_FOREACH(const size_t j, landmarkKeys) { + switch(dictionary[j]) { + case 0: C.push_back(j); break; + case 1: A.push_back(j); break; + case 2: B.push_back(j); break; + default: std::cout << j << ": " << dictionary[j] << std::endl; + throw std::runtime_error("addLandmarkToPartitionResult: wrong status for landmark"); + } + } + } #define REDUCE_CAMERA_GRAPH - /* ************************************************************************* */ - template - boost::optional findPartitoning(const GenericGraph& graph, const std::vector& keys, - WorkSpace& workspace, bool verbose, - const boost::optional >& int2symbol, const bool reduceGraph) { - boost::optional result; - GenericGraph reducedGraph; - std::vector keyToPartition; - std::vector cameraKeys, landmarkKeys; - if (reduceGraph) { - if (!int2symbol.is_initialized()) - throw std::invalid_argument("findSeparator: int2symbol must be valid!"); + /* ************************************************************************* */ + template + boost::optional findPartitoning(const GenericGraph& graph, const std::vector& keys, + WorkSpace& workspace, bool verbose, + const boost::optional >& int2symbol, const bool reduceGraph) { + boost::optional result; + GenericGraph reducedGraph; + std::vector keyToPartition; + std::vector cameraKeys, landmarkKeys; + if (reduceGraph) { + if (!int2symbol.is_initialized()) + throw std::invalid_argument("findSeparator: int2symbol must be valid!"); - // find out all the landmark keys, which are to be eliminated - cameraKeys.reserve(keys.size()); - landmarkKeys.reserve(keys.size()); - BOOST_FOREACH(const size_t key, keys) { - if((*int2symbol)[key].chr() == 'x') - cameraKeys.push_back(key); - else - landmarkKeys.push_back(key); - } + // find out all the landmark keys, which are to be eliminated + cameraKeys.reserve(keys.size()); + landmarkKeys.reserve(keys.size()); + BOOST_FOREACH(const size_t key, keys) { + if((*int2symbol)[key].chr() == 'x') + cameraKeys.push_back(key); + else + landmarkKeys.push_back(key); + } - keyToPartition = cameraKeys; - workspace.prepareDictionary(keyToPartition); - const std::vector& dictionary = workspace.dictionary; - reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph); - std::cout << "original graph: V" << keys.size() << ", E" << graph.size() - << " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << std::endl; - result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose); - } else // call Metis to partition the graph to A, B, C - result = separatorPartitionByMetis(graph, keys, workspace, verbose); + keyToPartition = cameraKeys; + workspace.prepareDictionary(keyToPartition); + const std::vector& dictionary = workspace.dictionary; + reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph); + std::cout << "original graph: V" << keys.size() << ", E" << graph.size() + << " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << std::endl; + result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose); + } else // call Metis to partition the graph to A, B, C + result = separatorPartitionByMetis(graph, keys, workspace, verbose); - if (!result.is_initialized()) { - std::cout << "metis failed!" << std::endl; - return 0; - } + if (!result.is_initialized()) { + std::cout << "metis failed!" << std::endl; + return 0; + } - if (reduceGraph) { - addLandmarkToPartitionResult(graph, landmarkKeys, *result, workspace); - std::cout << "the separator size: " << result->C.size() << " landmarks" << std::endl; - } + if (reduceGraph) { + addLandmarkToPartitionResult(graph, landmarkKeys, *result, workspace); + std::cout << "the separator size: " << result->C.size() << " landmarks" << std::endl; + } - return result; - } + return result; + } - /* ************************************************************************* */ - template - int findSeparator(const GenericGraph& graph, const std::vector& keys, - const int minNodesPerMap, WorkSpace& workspace, bool verbose, - const boost::optional >& int2symbol, const bool reduceGraph, - const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { + /* ************************************************************************* */ + template + int findSeparator(const GenericGraph& graph, const std::vector& keys, + const int minNodesPerMap, WorkSpace& workspace, bool verbose, + const boost::optional >& int2symbol, const bool reduceGraph, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { - boost::optional result = findPartitoning(graph, keys, workspace, - verbose, int2symbol, reduceGraph); + boost::optional result = findPartitoning(graph, keys, workspace, + verbose, int2symbol, reduceGraph); - // find the island in A and B, and make them separated submaps - typedef std::vector Island; - std::list islands; + // find the island in A and B, and make them separated submaps + typedef std::vector Island; + std::list islands; - std::list islands_in_A = findIslands(graph, result->A, workspace, - minNrConstraintsPerCamera, minNrConstraintsPerLandmark); + std::list islands_in_A = findIslands(graph, result->A, workspace, + minNrConstraintsPerCamera, minNrConstraintsPerLandmark); - std::list islands_in_B = findIslands(graph, result->B, workspace, - minNrConstraintsPerCamera, minNrConstraintsPerLandmark); + std::list islands_in_B = findIslands(graph, result->B, workspace, + minNrConstraintsPerCamera, minNrConstraintsPerLandmark); - 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.sort(isLargerIsland); - size_t numIsland0 = islands.size(); + 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.sort(isLargerIsland); + size_t numIsland0 = islands.size(); #ifdef NDEBUG -// verbose = true; -// if (!int2symbol) throw std::invalid_argument("findSeparator: int2symbol is not set!"); -// std::cout << "sep size: " << result->C.size() << "; "; -// printNumCamerasLandmarks(result->C, *int2symbol); -// std::cout << "no. of island: " << islands.size() << "; "; -// std::cout << "island size: "; -// BOOST_FOREACH(const Island& island, islands) -// std::cout << island.size() << " "; -// std::cout << std::endl; +// verbose = true; +// if (!int2symbol) throw std::invalid_argument("findSeparator: int2symbol is not set!"); +// std::cout << "sep size: " << result->C.size() << "; "; +// printNumCamerasLandmarks(result->C, *int2symbol); +// std::cout << "no. of island: " << islands.size() << "; "; +// std::cout << "island size: "; +// BOOST_FOREACH(const Island& island, islands) +// std::cout << island.size() << " "; +// std::cout << std::endl; -// BOOST_FOREACH(const Island& island, islands) { -// printNumCamerasLandmarks(island, int2symbol); -// } +// BOOST_FOREACH(const Island& island, islands) { +// printNumCamerasLandmarks(island, int2symbol); +// } #endif - // absorb small components into the separator - size_t oldSize = islands.size(); - while(true) { - if (islands.size() < 2) { - std::cout << "numIsland: " << numIsland0 << std::endl; - throw std::runtime_error("findSeparator: found fewer than 2 submaps!"); - } + // absorb small components into the separator + size_t oldSize = islands.size(); + while(true) { + if (islands.size() < 2) { + std::cout << "numIsland: " << numIsland0 << std::endl; + throw std::runtime_error("findSeparator: found fewer than 2 submaps!"); + } - std::list::reference island = islands.back(); - if ((int)island.size() >= minNodesPerMap) break; - result->C.insert(result->C.end(), island.begin(), island.end()); - islands.pop_back(); - } - if (islands.size() != oldSize){ - if (verbose) std::cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << std::endl; - } - else{ - if (verbose) std::cout << oldSize << " submap(s);\t" << std::endl; - } + std::list::reference island = islands.back(); + if ((int)island.size() >= minNodesPerMap) break; + result->C.insert(result->C.end(), island.begin(), island.end()); + islands.pop_back(); + } + if (islands.size() != oldSize){ + if (verbose) std::cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << std::endl; + } + else{ + if (verbose) std::cout << oldSize << " submap(s);\t" << std::endl; + } - // generate the node map - std::vector& partitionTable = workspace.partitionTable; - std::fill(partitionTable.begin(), partitionTable.end(), -1); - BOOST_FOREACH(const size_t key, result->C) - partitionTable[key] = 0; - int idx = 0; - BOOST_FOREACH(const Island& island, islands) { - idx++; - BOOST_FOREACH(const size_t key, island) { - partitionTable[key] = idx; - } - } + // generate the node map + std::vector& partitionTable = workspace.partitionTable; + std::fill(partitionTable.begin(), partitionTable.end(), -1); + BOOST_FOREACH(const size_t key, result->C) + partitionTable[key] = 0; + int idx = 0; + BOOST_FOREACH(const Island& island, islands) { + idx++; + BOOST_FOREACH(const size_t key, island) { + partitionTable[key] = idx; + } + } - return islands.size(); - } + return islands.size(); + } }} //namespace diff --git a/gtsam_unstable/partition/FindSeparator.h b/gtsam_unstable/partition/FindSeparator.h index e52d40a12..42d971a82 100644 --- a/gtsam_unstable/partition/FindSeparator.h +++ b/gtsam_unstable/partition/FindSeparator.h @@ -16,29 +16,29 @@ namespace gtsam { namespace partition { -// typedef std::map PartitionTable; // from the key to the partition: 0 - separator, > 1: submap id +// typedef std::map PartitionTable; // from the key to the partition: 0 - separator, > 1: submap id - /** the metis Nest dissection result */ - struct MetisResult { - std::vector A, B; // frontals - std::vector C; // separator - }; + /** the metis Nest dissection result */ + struct MetisResult { + std::vector A, B; // frontals + std::vector C; // separator + }; - /** - * 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) - */ - template - boost::optional separatorPartitionByMetis(const GenericGraph& graph, const std::vector& keys, - WorkSpace& workspace, bool verbose); + /** + * 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) + */ + template + boost::optional separatorPartitionByMetis(const GenericGraph& graph, const std::vector& keys, + WorkSpace& workspace, bool verbose); - /** - * 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. - */ - template - int findSeparator(const GenericGraph& graph, const std::vector& keys, - const int minNodesPerMap, WorkSpace& workspace, bool verbose, const boost::optional >& int2symbol, - const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); + /** + * 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. + */ + template + int findSeparator(const GenericGraph& graph, const std::vector& keys, + const int minNodesPerMap, WorkSpace& workspace, bool verbose, const boost::optional >& int2symbol, + const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); }} //namespace diff --git a/gtsam_unstable/partition/tests/testFindSeparator.cpp b/gtsam_unstable/partition/tests/testFindSeparator.cpp index 4943f5456..9bdf40026 100644 --- a/gtsam_unstable/partition/tests/testFindSeparator.cpp +++ b/gtsam_unstable/partition/tests/testFindSeparator.cpp @@ -185,45 +185,45 @@ TEST ( Partition, findSeparator2 ) // x25 x26 x27 x28 TEST ( Partition, findSeparator3_with_reduced_camera ) { - GenericGraph3D graph; - for (int j=1; j<=8; j++) - graph.push_back(boost::make_shared(25, j)); - for (int j=1; j<=16; j++) - graph.push_back(boost::make_shared(26, j)); - for (int j=9; j<=24; j++) - graph.push_back(boost::make_shared(27, j)); - for (int j=17; j<=24; j++) - graph.push_back(boost::make_shared(28, j)); + GenericGraph3D graph; + for (int j=1; j<=8; j++) + graph.push_back(boost::make_shared(25, j)); + for (int j=1; j<=16; j++) + graph.push_back(boost::make_shared(26, j)); + for (int j=9; j<=24; j++) + graph.push_back(boost::make_shared(27, j)); + for (int j=17; j<=24; j++) + graph.push_back(boost::make_shared(28, j)); - std::vector keys; - for(int i=1; i<=28; i++) - keys.push_back(i); + std::vector keys; + for(int i=1; i<=28; i++) + keys.push_back(i); - vector int2symbol; - int2symbol.push_back(Symbol('x',0)); // dummy - for(int i=1; i<=24; i++) - int2symbol.push_back(Symbol('l',i)); - int2symbol.push_back(Symbol('x',25)); - int2symbol.push_back(Symbol('x',26)); - int2symbol.push_back(Symbol('x',27)); - int2symbol.push_back(Symbol('x',28)); + vector int2symbol; + int2symbol.push_back(Symbol('x',0)); // dummy + for(int i=1; i<=24; i++) + int2symbol.push_back(Symbol('l',i)); + int2symbol.push_back(Symbol('x',25)); + int2symbol.push_back(Symbol('x',26)); + int2symbol.push_back(Symbol('x',27)); + int2symbol.push_back(Symbol('x',28)); - WorkSpace workspace(29); - bool reduceGraph = true; - int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph, 0, 0); - LONGS_EQUAL(2, numIsland); + WorkSpace workspace(29); + bool reduceGraph = true; + int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph, 0, 0); + LONGS_EQUAL(2, numIsland); - partition::PartitionTable& partitionTable = workspace.partitionTable; - for (int j=1; j<=8; j++) - LONGS_EQUAL(1, partitionTable[j]); - for (int j=9; j<=16; j++) - LONGS_EQUAL(0, partitionTable[j]); - for (int j=17; j<=24; j++) - LONGS_EQUAL(2, partitionTable[j]); - LONGS_EQUAL(1, partitionTable[25]); - LONGS_EQUAL(1, partitionTable[26]); - LONGS_EQUAL(2, partitionTable[27]); - LONGS_EQUAL(2, partitionTable[28]); + partition::PartitionTable& partitionTable = workspace.partitionTable; + for (int j=1; j<=8; j++) + LONGS_EQUAL(1, partitionTable[j]); + for (int j=9; j<=16; j++) + LONGS_EQUAL(0, partitionTable[j]); + for (int j=17; j<=24; j++) + LONGS_EQUAL(2, partitionTable[j]); + LONGS_EQUAL(1, partitionTable[25]); + LONGS_EQUAL(1, partitionTable[26]); + LONGS_EQUAL(2, partitionTable[27]); + LONGS_EQUAL(2, partitionTable[28]); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index 8ff61737f..ff7307840 100644 --- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -121,3 +121,4 @@ int main() TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ + diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index 2a7a572d5..3a0081bdb 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -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);} /* ************************************************************************* */ diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 7fbee3a5b..6d8a056fc 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -273,11 +273,11 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { // test convergence Values actual = optimizer.optimize(); - EXPECT(assert_equal(expected, actual, 1e-6)); + EXPECT(assert_equal(expected, actual)); // Check that the gradient is zero 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()));