From 699153ece9fd4c8dcbae5ed9b17e3c32da97f1e9 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Thu, 30 Oct 2014 12:44:46 -0400 Subject: [PATCH] Coding convention: convert tabs to two spaces --- gtsam/base/LieMatrix.h | 2 +- gtsam/linear/GaussianFactorGraph.cpp | 6 +- gtsam/linear/HessianFactor.cpp | 2 +- gtsam/navigation/CombinedImuFactor.h | 96 +- gtsam/navigation/ImuFactor.h | 74 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 64 +- gtsam_unstable/base/DSFMap.h | 138 +-- .../discrete/tests/testLoopyBelief.cpp | 4 +- gtsam_unstable/partition/FindSeparator-inl.h | 910 +++++++++--------- gtsam_unstable/partition/FindSeparator.h | 42 +- gtsam_unstable/partition/GenericGraph.cpp | 792 +++++++-------- gtsam_unstable/partition/GenericGraph.h | 216 ++--- .../partition/NestedDissection-inl.h | 404 ++++---- gtsam_unstable/partition/NestedDissection.h | 80 +- gtsam_unstable/partition/PartitionWorkSpace.h | 44 +- .../partition/tests/testFindSeparator.cpp | 70 +- .../partition/tests/testGenericGraph.cpp | 270 +++--- .../partition/tests/testNestedDissection.cpp | 434 ++++----- gtsam_unstable/slam/BetweenFactorEM.h | 100 +- .../slam/GaussMarkov1stOrderFactor.h | 8 +- .../slam/tests/testBetweenFactorEM.cpp | 54 +- 21 files changed, 1905 insertions(+), 1905 deletions(-) diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 23e5fd023..ec2dec815 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -157,7 +157,7 @@ struct LieMatrix : public Matrix, public DerivedValue { result.data(), p.rows(), p.cols()) = p; return result; } - + /// @} private: diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 438a06783..54e721cd7 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -85,7 +85,7 @@ namespace gtsam { dims_accumulated.resize(dims.size()+1,0); dims_accumulated[0]=0; for (size_t i=1; i FactorKeys = getkeydim(); - BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) + vector FactorKeys = getkeydim(); + BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) f->multiplyHessianAdd(alpha, x, y, FactorKeys); } diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 58aee8073..f282682b3 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -538,7 +538,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, // copy to yvalues for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) { - bool didNotExist; + bool didNotExist; VectorValues::iterator it; boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector()); if (didNotExist) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 725274acc..8c53dbf93 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -345,12 +345,12 @@ namespace gtsam { /** Constructor */ CombinedImuFactor( - Key pose_i, ///< previous pose key - Key vel_i, ///< previous velocity key - Key pose_j, ///< current pose key - Key vel_j, ///< current velocity key - Key bias_i, ///< previous bias key - Key bias_j, ///< current bias key + Key pose_i, ///< previous pose key + Key vel_i, ///< previous velocity key + Key pose_j, ///< current pose key + Key vel_j, ///< current velocity key + Key bias_i, ///< previous bias key + Key bias_j, ///< current bias key const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements const Vector3& gravity, ///< gravity vector const Vector3& omegaCoriolis, ///< rotation rate of inertial frame @@ -480,33 +480,33 @@ namespace gtsam { Matrix3 dfPdPi; Matrix3 dfVdPi; if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; } else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Matrix3::Zero(); + dfPdPi = - Rot_i.matrix(); + dfVdPi = Matrix3::Zero(); } - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(), - //dBiasAcc/dPi - Matrix3::Zero(), Matrix3::Zero(), - //dBiasOmega/dPi - Matrix3::Zero(), Matrix3::Zero(); + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(), + //dBiasAcc/dPi + Matrix3::Zero(), Matrix3::Zero(), + //dBiasOmega/dPi + Matrix3::Zero(), Matrix3::Zero(); } if(H2) { @@ -517,13 +517,13 @@ namespace gtsam { + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(), - //dBiasAcc/dVi - Matrix3::Zero(), - //dBiasOmega/dVi - Matrix3::Zero(); + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(), + //dBiasAcc/dVi + Matrix3::Zero(), + //dBiasOmega/dVi + Matrix3::Zero(); } if(H3) { @@ -643,21 +643,21 @@ namespace gtsam { // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij - + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; + + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij - + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity } const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index d68af4f8b..b95f9c346 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -308,11 +308,11 @@ namespace gtsam { /** Constructor */ ImuFactor( - Key pose_i, ///< previous pose key - Key vel_i, ///< previous velocity key - Key pose_j, ///< current pose key - Key vel_j, ///< current velocity key - Key bias, ///< previous bias key + Key pose_i, ///< previous pose key + Key vel_i, ///< previous velocity key + Key pose_j, ///< current pose key + Key vel_j, ///< current velocity key + Key bias, ///< previous bias key const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements const Vector3& gravity, ///< gravity vector const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame @@ -419,29 +419,29 @@ namespace gtsam { Matrix3 dfPdPi; Matrix3 dfVdPi; if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; } else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Matrix3::Zero(); + dfPdPi = - Rot_i.matrix(); + dfVdPi = Matrix3::Zero(); } - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(); + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(); } if(H2) { @@ -540,22 +540,22 @@ namespace gtsam { // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij - + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; + Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij + + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij - + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity } const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 63a1a2218..3bbb63b88 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -176,11 +176,11 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, stm << "];\n"; if (firstTimePoses) { - lastKey = key; - firstTimePoses = false; + lastKey = key; + firstTimePoses = false; } else { - stm << " var" << key << "--" << "var" << lastKey << ";\n"; - lastKey = key; + stm << " var" << key << "--" << "var" << lastKey << ";\n"; + lastKey = key; } } stm << "\n"; @@ -219,37 +219,37 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Create factors and variable connections for(size_t i = 0; i < this->size(); ++i) { if(graphvizFormatting.plotFactorPoints){ - // Make each factor a dot - stm << " factor" << i << "[label=\"\", shape=point"; - { - map::const_iterator pos = graphvizFormatting.factorPositions.find(i); - if(pos != graphvizFormatting.factorPositions.end()) - stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; - } - stm << "];\n"; + // Make each factor a dot + stm << " factor" << i << "[label=\"\", shape=point"; + { + map::const_iterator pos = graphvizFormatting.factorPositions.find(i); + if(pos != graphvizFormatting.factorPositions.end()) + stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; + } + stm << "];\n"; - // Make factor-variable connections - if(graphvizFormatting.connectKeysToFactor && this->at(i)) { - BOOST_FOREACH(Key key, *this->at(i)) { - stm << " var" << key << "--" << "factor" << i << ";\n"; - } - } + // Make factor-variable connections + if(graphvizFormatting.connectKeysToFactor && this->at(i)) { + BOOST_FOREACH(Key key, *this->at(i)) { + stm << " var" << key << "--" << "factor" << i << ";\n"; + } + } - } + } else { - if(this->at(i)) { - Key k; - bool firstTime = true; - BOOST_FOREACH(Key key, *this->at(i)) { - if(firstTime){ - k = key; - firstTime = false; - continue; - } - stm << " var" << key << "--" << "var" << k << ";\n"; - k = key; - } - } + if(this->at(i)) { + Key k; + bool firstTime = true; + BOOST_FOREACH(Key key, *this->at(i)) { + if(firstTime){ + k = key; + firstTime = false; + continue; + } + stm << " var" << key << "--" << "var" << k << ";\n"; + k = key; + } + } } } } diff --git a/gtsam_unstable/base/DSFMap.h b/gtsam_unstable/base/DSFMap.h index 6832c7f83..6ddb74cfd 100644 --- a/gtsam_unstable/base/DSFMap.h +++ b/gtsam_unstable/base/DSFMap.h @@ -34,89 +34,89 @@ class DSFMap { protected: - /// We store the forest in an STL map, but parents are done with pointers - struct Entry { - typename std::map::iterator parent_; - size_t rank_; - Entry() {} - }; + /// We store the forest in an STL map, but parents are done with pointers + struct Entry { + typename std::map::iterator parent_; + size_t rank_; + Entry() {} + }; typedef typename std::map Map; - typedef typename Map::iterator iterator; - mutable Map entries_; + typedef typename Map::iterator iterator; + mutable Map entries_; - /// Given key, find iterator to initial entry - iterator find__(const KEY& key) const { - static const Entry empty; - iterator it = entries_.find(key); - // if key does not exist, create and return itself - if (it == entries_.end()) { - it = entries_.insert(std::make_pair(key, empty)).first; - it->second.parent_ = it; - it->second.rank_ = 0; - } - return it; - } + /// Given key, find iterator to initial entry + iterator find__(const KEY& key) const { + static const Entry empty; + iterator it = entries_.find(key); + // if key does not exist, create and return itself + if (it == entries_.end()) { + it = entries_.insert(std::make_pair(key, empty)).first; + it->second.parent_ = it; + it->second.rank_ = 0; + } + return it; + } - /// Given iterator to initial entry, find the root Entry - iterator find_(const iterator& it) const { - // follow parent pointers until we reach set representative - iterator& parent = it->second.parent_; - if (parent != it) - parent = find_(parent); // not yet, recurse! - return parent; - } + /// Given iterator to initial entry, find the root Entry + iterator find_(const iterator& it) const { + // follow parent pointers until we reach set representative + iterator& parent = it->second.parent_; + if (parent != it) + parent = find_(parent); // not yet, recurse! + return parent; + } - /// Given key, find the root Entry - inline iterator find_(const KEY& key) const { - iterator initial = find__(key); - return find_(initial); - } + /// Given key, find the root Entry + inline iterator find_(const KEY& key) const { + iterator initial = find__(key); + return find_(initial); + } public: - typedef std::set Set; + typedef std::set Set; - /// constructor - DSFMap() { - } + /// constructor + DSFMap() { + } - /// Given key, find the representative key for the set in which it lives - inline KEY find(const KEY& key) const { - iterator root = find_(key); - return root->first; - } + /// Given key, find the representative key for the set in which it lives + inline KEY find(const KEY& key) const { + iterator root = find_(key); + return root->first; + } - /// Merge two sets - void merge(const KEY& x, const KEY& y) { + /// Merge two sets + void merge(const KEY& x, const KEY& y) { - // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure - iterator xRoot = find_(x); - iterator yRoot = find_(y); - if (xRoot == yRoot) - return; + // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure + iterator xRoot = find_(x); + iterator yRoot = find_(y); + if (xRoot == yRoot) + return; - // Merge sets - if (xRoot->second.rank_ < yRoot->second.rank_) - xRoot->second.parent_ = yRoot; - else if (xRoot->second.rank_ > yRoot->second.rank_) - yRoot->second.parent_ = xRoot; - else { - yRoot->second.parent_ = xRoot; - xRoot->second.rank_ = xRoot->second.rank_ + 1; - } - } + // Merge sets + if (xRoot->second.rank_ < yRoot->second.rank_) + xRoot->second.parent_ = yRoot; + else if (xRoot->second.rank_ > yRoot->second.rank_) + yRoot->second.parent_ = xRoot; + else { + yRoot->second.parent_ = xRoot; + xRoot->second.rank_ = xRoot->second.rank_ + 1; + } + } - /// return all sets, i.e. a partition of all elements - std::map sets() const { - std::map sets; - iterator it = entries_.begin(); - for (; it != entries_.end(); it++) { - iterator root = find_(it); - sets[root->first].insert(it->first); - } - return sets; - } + /// return all sets, i.e. a partition of all elements + std::map sets() const { + std::map sets; + iterator it = entries_.begin(); + for (; it != entries_.end(); it++) { + iterator root = find_(it); + sets[root->first].insert(it->first); + } + return sets; + } }; diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index 6dd9dd1b5..b579364b6 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -1,8 +1,8 @@ /** - * @file testLoopyBelief.cpp + * @file testLoopyBelief.cpp * @brief * @author Duy-Nguyen Ta - * @date Oct 11, 2013 + * @date Oct 11, 2013 */ #include diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index dacbebc76..849cf7a05 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -26,539 +26,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/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp index a3db6a9c1..b78c1d3dc 100644 --- a/gtsam_unstable/partition/GenericGraph.cpp +++ b/gtsam_unstable/partition/GenericGraph.cpp @@ -19,459 +19,459 @@ using namespace std; namespace gtsam { namespace partition { - /** - * Note: Need to be able to handle a graph with factors that involve variables not in the given {keys} - */ - list > findIslands(const GenericGraph2D& graph, const vector& keys, WorkSpace& workspace, - const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) - { - typedef pair IntPair; - typedef list FactorList; - typedef map Connections; + /** + * Note: Need to be able to handle a graph with factors that involve variables not in the given {keys} + */ + list > findIslands(const GenericGraph2D& graph, const vector& keys, WorkSpace& workspace, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) + { + typedef pair IntPair; + typedef list FactorList; + typedef map Connections; - // create disjoin set forest - DSFVector dsf(workspace.dsf, keys); + // create disjoin set forest + DSFVector dsf(workspace.dsf, keys); - FactorList factors(graph.begin(), graph.end()); - size_t nrFactors = factors.size(); - FactorList::iterator itEnd; - workspace.prepareDictionary(keys); - while (nrFactors) { - Connections connections; - bool succeed = false; - itEnd = factors.end(); - list toErase; - for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) { + FactorList factors(graph.begin(), graph.end()); + size_t nrFactors = factors.size(); + FactorList::iterator itEnd; + workspace.prepareDictionary(keys); + while (nrFactors) { + Connections connections; + bool succeed = false; + itEnd = factors.end(); + list toErase; + for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) { - // remove invalid factors - GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2; - if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) { - toErase.push_back(itFactor); nrFactors--; continue; - } + // remove invalid factors + GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2; + if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) { + toErase.push_back(itFactor); nrFactors--; continue; + } - size_t label1 = dsf.findSet(key1.index); - size_t label2 = dsf.findSet(key2.index); - if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } + size_t label1 = dsf.findSet(key1.index); + size_t label2 = dsf.findSet(key2.index); + if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } - // merge two trees if the connection is strong enough, otherwise cache it - // an odometry factor always merges two islands - if (key1.type == NODE_POSE_2D && key2.type == NODE_POSE_2D) { - toErase.push_back(itFactor); nrFactors--; - dsf.makeUnionInPlace(label1, label2); - succeed = true; - break; - } + // merge two trees if the connection is strong enough, otherwise cache it + // an odometry factor always merges two islands + if (key1.type == NODE_POSE_2D && key2.type == NODE_POSE_2D) { + toErase.push_back(itFactor); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + succeed = true; + break; + } - // single landmark island only need one measurement - if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) || - (dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) { - toErase.push_back(itFactor); nrFactors--; - dsf.makeUnionInPlace(label1, label2); - succeed = true; - break; - } + // single landmark island only need one measurement + if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) || + (dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) { + toErase.push_back(itFactor); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + succeed = true; + break; + } - // stack the current factor with the cached constraint - IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1); - Connections::iterator itCached = connections.find(labels); - if (itCached == connections.end()) { - connections.insert(make_pair(labels, itFactor)); - continue; - } else { - GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2; - // if observe the same landmark, we can not merge, abandon the current factor - if ((key1.index == key21.index && key1.type == NODE_LANDMARK_2D) || - (key1.index == key22.index && key1.type == NODE_LANDMARK_2D) || - (key2.index == key21.index && key2.type == NODE_LANDMARK_2D) || - (key2.index == key22.index && key2.type == NODE_LANDMARK_2D)) { - toErase.push_back(itFactor); nrFactors--; - continue; - } else { - toErase.push_back(itFactor); nrFactors--; - toErase.push_back(itCached->second); nrFactors--; - dsf.makeUnionInPlace(label1, label2); - connections.erase(itCached); - succeed = true; - break; - } - } - } + // stack the current factor with the cached constraint + IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1); + Connections::iterator itCached = connections.find(labels); + if (itCached == connections.end()) { + connections.insert(make_pair(labels, itFactor)); + continue; + } else { + GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2; + // if observe the same landmark, we can not merge, abandon the current factor + if ((key1.index == key21.index && key1.type == NODE_LANDMARK_2D) || + (key1.index == key22.index && key1.type == NODE_LANDMARK_2D) || + (key2.index == key21.index && key2.type == NODE_LANDMARK_2D) || + (key2.index == key22.index && key2.type == NODE_LANDMARK_2D)) { + toErase.push_back(itFactor); nrFactors--; + continue; + } else { + toErase.push_back(itFactor); nrFactors--; + toErase.push_back(itCached->second); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + connections.erase(itCached); + succeed = true; + break; + } + } + } - // erase unused factors - BOOST_FOREACH(const FactorList::iterator& it, toErase) - factors.erase(it); + // erase unused factors + BOOST_FOREACH(const FactorList::iterator& it, toErase) + factors.erase(it); - if (!succeed) break; - } + if (!succeed) break; + } - list > islands; - map > arrays = dsf.arrays(); - size_t key; vector array; - BOOST_FOREACH(boost::tie(key, array), arrays) - islands.push_back(array); - return islands; - } + list > islands; + map > arrays = dsf.arrays(); + size_t key; vector array; + BOOST_FOREACH(boost::tie(key, array), arrays) + islands.push_back(array); + return islands; + } - /* ************************************************************************* */ - void print(const GenericGraph2D& graph, const std::string name) { - cout << name << endl; - BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph) - cout << factor_->key1.index << " " << factor_->key2.index << endl; - } + /* ************************************************************************* */ + void print(const GenericGraph2D& graph, const std::string name) { + cout << name << endl; + BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph) + cout << factor_->key1.index << " " << factor_->key2.index << endl; + } - /* ************************************************************************* */ - void print(const GenericGraph3D& graph, const std::string name) { - cout << name << endl; - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) - cout << factor_->key1.index << " " << factor_->key2.index << " (" << - factor_->key1.type << ", " << factor_->key2.type <<")" << endl; - } + /* ************************************************************************* */ + void print(const GenericGraph3D& graph, const std::string name) { + cout << name << endl; + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) + cout << factor_->key1.index << " " << factor_->key2.index << " (" << + factor_->key1.type << ", " << factor_->key2.type <<")" << endl; + } - /* ************************************************************************* */ - // create disjoin set forest - DSFVector createDSF(const GenericGraph3D& graph, const vector& keys, const WorkSpace& workspace) { - DSFVector dsf(workspace.dsf, keys); - typedef list FactorList; + /* ************************************************************************* */ + // create disjoin set forest + DSFVector createDSF(const GenericGraph3D& graph, const vector& keys, const WorkSpace& workspace) { + DSFVector dsf(workspace.dsf, keys); + typedef list FactorList; - FactorList factors(graph.begin(), graph.end()); - size_t nrFactors = factors.size(); - FactorList::iterator itEnd; - while (nrFactors) { + FactorList factors(graph.begin(), graph.end()); + size_t nrFactors = factors.size(); + FactorList::iterator itEnd; + while (nrFactors) { - bool succeed = false; - itEnd = factors.end(); - list toErase; - for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) { + bool succeed = false; + itEnd = factors.end(); + list toErase; + for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) { - // remove invalid factors - if (graph.size() == 178765) cout << "kai21" << endl; - GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2; - if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl; - if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) { - toErase.push_back(itFactor); nrFactors--; continue; - } + // remove invalid factors + if (graph.size() == 178765) cout << "kai21" << endl; + GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2; + if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl; + if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) { + toErase.push_back(itFactor); nrFactors--; continue; + } - if (graph.size() == 178765) cout << "kai22" << endl; - size_t label1 = dsf.findSet(key1.index); - size_t label2 = dsf.findSet(key2.index); - if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } + if (graph.size() == 178765) cout << "kai22" << endl; + size_t label1 = dsf.findSet(key1.index); + size_t label2 = dsf.findSet(key2.index); + if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } - if (graph.size() == 178765) cout << "kai23" << endl; - // merge two trees if the connection is strong enough, otherwise cache it - // an odometry factor always merges two islands - if ((key1.type == NODE_POSE_3D && key2.type == NODE_LANDMARK_3D) || - (key1.type == NODE_POSE_3D && key2.type == NODE_POSE_3D)) { - toErase.push_back(itFactor); nrFactors--; - dsf.makeUnionInPlace(label1, label2); - succeed = true; - break; - } + if (graph.size() == 178765) cout << "kai23" << endl; + // merge two trees if the connection is strong enough, otherwise cache it + // an odometry factor always merges two islands + if ((key1.type == NODE_POSE_3D && key2.type == NODE_LANDMARK_3D) || + (key1.type == NODE_POSE_3D && key2.type == NODE_POSE_3D)) { + toErase.push_back(itFactor); nrFactors--; + dsf.makeUnionInPlace(label1, label2); + succeed = true; + break; + } - if (graph.size() == 178765) cout << "kai24" << endl; + if (graph.size() == 178765) cout << "kai24" << endl; - } + } - // erase unused factors - BOOST_FOREACH(const FactorList::iterator& it, toErase) - factors.erase(it); + // erase unused factors + BOOST_FOREACH(const FactorList::iterator& it, toErase) + factors.erase(it); - if (!succeed) break; - } - return dsf; - } + if (!succeed) break; + } + return dsf; + } - /* ************************************************************************* */ - // first check the type of the key (pose or landmark), and then check whether it is singular - inline bool isSingular(const set& singularCameras, const set& singularLandmarks, const GenericNode3D& node) { - switch(node.type) { - case NODE_POSE_3D: - return singularCameras.find(node.index) != singularCameras.end(); break; - case NODE_LANDMARK_3D: - return singularLandmarks.find(node.index) != singularLandmarks.end(); break; - default: - throw runtime_error("unrecognized key type!"); - } - } + /* ************************************************************************* */ + // first check the type of the key (pose or landmark), and then check whether it is singular + inline bool isSingular(const set& singularCameras, const set& singularLandmarks, const GenericNode3D& node) { + switch(node.type) { + case NODE_POSE_3D: + return singularCameras.find(node.index) != singularCameras.end(); break; + case NODE_LANDMARK_3D: + return singularLandmarks.find(node.index) != singularLandmarks.end(); break; + default: + throw runtime_error("unrecognized key type!"); + } + } - /* ************************************************************************* */ - void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace, - const vector& isCamera, const vector& isLandmark, - set& singularCameras, set& singularLandmarks, vector& nrConstraints, - bool& foundSingularCamera, bool& foundSingularLandmark, - const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { + /* ************************************************************************* */ + void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace, + const vector& isCamera, const vector& isLandmark, + set& singularCameras, set& singularLandmarks, vector& nrConstraints, + bool& foundSingularCamera, bool& foundSingularLandmark, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { - // compute the constraint number per camera - std::fill(nrConstraints.begin(), nrConstraints.end(), 0); - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { - const int& key1 = factor_->key1.index; - const int& key2 = factor_->key2.index; - if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 && - !isSingular(singularCameras, singularLandmarks, factor_->key1) && - !isSingular(singularCameras, singularLandmarks, factor_->key2)) { - nrConstraints[key1]++; - nrConstraints[key2]++; + // compute the constraint number per camera + std::fill(nrConstraints.begin(), nrConstraints.end(), 0); + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + const int& key1 = factor_->key1.index; + const int& key2 = factor_->key2.index; + if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 && + !isSingular(singularCameras, singularLandmarks, factor_->key1) && + !isSingular(singularCameras, singularLandmarks, factor_->key2)) { + nrConstraints[key1]++; + nrConstraints[key2]++; - // a single pose constraint is sufficient for stereo, so we add 2 to the counter - // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera - if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){ - nrConstraints[key1]+=2; - nrConstraints[key2]+=2; - } - } - } + // a single pose constraint is sufficient for stereo, so we add 2 to the counter + // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera + if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){ + nrConstraints[key1]+=2; + nrConstraints[key2]+=2; + } + } + } - // find singular cameras and landmarks - foundSingularCamera = false; - foundSingularLandmark = false; - for (size_t i=0; i > findIslands(const GenericGraph3D& graph, const vector& keys, WorkSpace& workspace, - const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) { + /* ************************************************************************* */ + list > findIslands(const GenericGraph3D& graph, const vector& keys, WorkSpace& workspace, + const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) { - // create disjoint set forest - workspace.prepareDictionary(keys); - DSFVector dsf = createDSF(graph, keys, workspace); + // create disjoint set forest + workspace.prepareDictionary(keys); + DSFVector dsf = createDSF(graph, keys, workspace); - const bool verbose = false; - bool foundSingularCamera = true; - bool foundSingularLandmark = true; + const bool verbose = false; + bool foundSingularCamera = true; + bool foundSingularLandmark = true; - list > islands; - set singularCameras, singularLandmarks; - vector isCamera(workspace.dictionary.size(), false); - vector isLandmark(workspace.dictionary.size(), false); + list > islands; + set singularCameras, singularLandmarks; + vector isCamera(workspace.dictionary.size(), false); + vector isLandmark(workspace.dictionary.size(), false); - // check the constraint number of every variable - // find the camera and landmark keys - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { - //assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM - if (workspace.dictionary[factor_->key1.index] != -1) { - if (factor_->key1.type == NODE_POSE_3D) - isCamera[factor_->key1.index] = true; - else - isLandmark[factor_->key1.index] = true; - } + // check the constraint number of every variable + // find the camera and landmark keys + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + //assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM + if (workspace.dictionary[factor_->key1.index] != -1) { + if (factor_->key1.type == NODE_POSE_3D) + isCamera[factor_->key1.index] = true; + else + isLandmark[factor_->key1.index] = true; + } if (workspace.dictionary[factor_->key2.index] != -1) { - if (factor_->key2.type == NODE_POSE_3D) - isCamera[factor_->key2.index] = true; - else - isLandmark[factor_->key2.index] = true; + if (factor_->key2.type == NODE_POSE_3D) + isCamera[factor_->key2.index] = true; + else + isLandmark[factor_->key2.index] = true; } - } + } - vector nrConstraints(workspace.dictionary.size(), 0); - // iterate until all singular variables have been removed. Removing a singular variable - // can cause another to become singular, so this will probably run several times - while (foundSingularCamera || foundSingularLandmark) { - findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark, // input - singularCameras, singularLandmarks, nrConstraints, // output - foundSingularCamera, foundSingularLandmark, // output - minNrConstraintsPerCamera, minNrConstraintsPerLandmark); // input - } + vector nrConstraints(workspace.dictionary.size(), 0); + // iterate until all singular variables have been removed. Removing a singular variable + // can cause another to become singular, so this will probably run several times + while (foundSingularCamera || foundSingularLandmark) { + findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark, // input + singularCameras, singularLandmarks, nrConstraints, // output + foundSingularCamera, foundSingularLandmark, // output + minNrConstraintsPerCamera, minNrConstraintsPerLandmark); // input + } - // add singular variables directly as islands - if (!singularCameras.empty()) { - if (verbose) cout << "singular cameras:"; - BOOST_FOREACH(const size_t i, singularCameras) { - islands.push_back(vector(1, i)); // <--------------------------- - if (verbose) cout << i << " "; - } - if (verbose) cout << endl; - } - if (!singularLandmarks.empty()) { - if (verbose) cout << "singular landmarks:"; - BOOST_FOREACH(const size_t i, singularLandmarks) { - islands.push_back(vector(1, i)); // <--------------------------- - if (verbose) cout << i << " "; - } - if (verbose) cout << endl; - } + // add singular variables directly as islands + if (!singularCameras.empty()) { + if (verbose) cout << "singular cameras:"; + BOOST_FOREACH(const size_t i, singularCameras) { + islands.push_back(vector(1, i)); // <--------------------------- + if (verbose) cout << i << " "; + } + if (verbose) cout << endl; + } + if (!singularLandmarks.empty()) { + if (verbose) cout << "singular landmarks:"; + BOOST_FOREACH(const size_t i, singularLandmarks) { + islands.push_back(vector(1, i)); // <--------------------------- + if (verbose) cout << i << " "; + } + if (verbose) cout << endl; + } - // regenerating islands - map > labelIslands = dsf.arrays(); - size_t label; vector island; - BOOST_FOREACH(boost::tie(label, island), labelIslands) { - vector filteredIsland; // remove singular cameras from array - filteredIsland.reserve(island.size()); - BOOST_FOREACH(const size_t key, island) { - if ((isCamera[key] && singularCameras.find(key) == singularCameras.end()) || // not singular - (isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) || // not singular - (!isCamera[key] && !isLandmark[key])) { // the key is not involved in any factor, so the type is undertermined - filteredIsland.push_back(key); - } - } - islands.push_back(filteredIsland); - } + // regenerating islands + map > labelIslands = dsf.arrays(); + size_t label; vector island; + BOOST_FOREACH(boost::tie(label, island), labelIslands) { + vector filteredIsland; // remove singular cameras from array + filteredIsland.reserve(island.size()); + BOOST_FOREACH(const size_t key, island) { + if ((isCamera[key] && singularCameras.find(key) == singularCameras.end()) || // not singular + (isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) || // not singular + (!isCamera[key] && !isLandmark[key])) { // the key is not involved in any factor, so the type is undertermined + filteredIsland.push_back(key); + } + } + islands.push_back(filteredIsland); + } - // sanity check - size_t nrKeys = 0; - BOOST_FOREACH(const vector& island, islands) - nrKeys += island.size(); - if (nrKeys != keys.size()) { - cout << nrKeys << " vs " << keys.size() << endl; - throw runtime_error("findIslands: the number of keys is inconsistent!"); - } + // sanity check + size_t nrKeys = 0; + BOOST_FOREACH(const vector& island, islands) + nrKeys += island.size(); + if (nrKeys != keys.size()) { + cout << nrKeys << " vs " << keys.size() << endl; + throw runtime_error("findIslands: the number of keys is inconsistent!"); + } - if (verbose) cout << "found " << islands.size() << " islands!" << endl; - return islands; - } + if (verbose) cout << "found " << islands.size() << " islands!" << endl; + return islands; + } - /* ************************************************************************* */ - // return the number of intersection between two **sorted** landmark vectors - inline int getNrCommonLandmarks(const vector& landmarks1, const vector& landmarks2){ - size_t i1 = 0, i2 = 0; - int nrCommonLandmarks = 0; - while (i1 < landmarks1.size() && i2 < landmarks2.size()) { - if (landmarks1[i1] < landmarks2[i2]) - i1 ++; - else if (landmarks1[i1] > landmarks2[i2]) - i2 ++; - else { - i1++; i2++; - nrCommonLandmarks ++; - } - } - return nrCommonLandmarks; - } + /* ************************************************************************* */ + // return the number of intersection between two **sorted** landmark vectors + inline int getNrCommonLandmarks(const vector& landmarks1, const vector& landmarks2){ + size_t i1 = 0, i2 = 0; + int nrCommonLandmarks = 0; + while (i1 < landmarks1.size() && i2 < landmarks2.size()) { + if (landmarks1[i1] < landmarks2[i2]) + i1 ++; + else if (landmarks1[i1] > landmarks2[i2]) + i2 ++; + else { + i1++; i2++; + nrCommonLandmarks ++; + } + } + return nrCommonLandmarks; + } - /* ************************************************************************* */ - void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, - const std::vector& dictionary, GenericGraph3D& reducedGraph) { + /* ************************************************************************* */ + void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, + const std::vector& dictionary, GenericGraph3D& reducedGraph) { - typedef size_t CameraKey; - typedef pair CameraPair; - typedef size_t LandmarkKey; - // get a mapping from each landmark to its connected cameras - vector > cameraToLandmarks(dictionary.size()); - // for odometry xi-xj where i cameraToCamera(dictionary.size(), -1); - size_t key_i, key_j; - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { - if (factor_->key1.type == NODE_POSE_3D) { - if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor - cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index); - } - else { // odometry factor - if (factor_->key1.index < factor_->key2.index) { - key_i = factor_->key1.index; - key_j = factor_->key2.index; - } else { - key_i = factor_->key2.index; - key_j = factor_->key1.index; - } - cameraToCamera[key_i] = key_j; - } - } - } + typedef size_t CameraKey; + typedef pair CameraPair; + typedef size_t LandmarkKey; + // get a mapping from each landmark to its connected cameras + vector > cameraToLandmarks(dictionary.size()); + // for odometry xi-xj where i cameraToCamera(dictionary.size(), -1); + size_t key_i, key_j; + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + if (factor_->key1.type == NODE_POSE_3D) { + if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor + cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index); + } + else { // odometry factor + if (factor_->key1.index < factor_->key2.index) { + key_i = factor_->key1.index; + key_j = factor_->key2.index; + } else { + key_i = factor_->key2.index; + key_j = factor_->key1.index; + } + cameraToCamera[key_i] = key_j; + } + } + } - // sort the landmark keys for the late getNrCommonLandmarks call - BOOST_FOREACH(vector &landmarks, cameraToLandmarks){ - if (!landmarks.empty()) - std::sort(landmarks.begin(), landmarks.end()); - } + // sort the landmark keys for the late getNrCommonLandmarks call + BOOST_FOREACH(vector &landmarks, cameraToLandmarks){ + if (!landmarks.empty()) + std::sort(landmarks.begin(), landmarks.end()); + } - // generate the reduced graph - reducedGraph.clear(); - int factorIndex = 0; - int camera1, camera2, nrTotalConstraints; - bool hasOdometry; - for (size_t i1=0; i1 0 || hasOdometry) { - nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0); - reducedGraph.push_back(boost::make_shared(camera1, camera2, - factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints)); - } - } - } - } + // generate the reduced graph + reducedGraph.clear(); + int factorIndex = 0; + int camera1, camera2, nrTotalConstraints; + bool hasOdometry; + for (size_t i1=0; i1 0 || hasOdometry) { + nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0); + reducedGraph.push_back(boost::make_shared(camera1, camera2, + factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints)); + } + } + } + } - /* ************************************************************************* */ - void checkSingularity(const GenericGraph3D& graph, const std::vector& frontals, - WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) { - workspace.prepareDictionary(frontals); - vector nrConstraints(workspace.dictionary.size(), 0); + /* ************************************************************************* */ + void checkSingularity(const GenericGraph3D& graph, const std::vector& frontals, + WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) { + workspace.prepareDictionary(frontals); + vector nrConstraints(workspace.dictionary.size(), 0); - // summarize the constraint number - const vector& dictionary = workspace.dictionary; - vector isValidCamera(workspace.dictionary.size(), false); - vector isValidLandmark(workspace.dictionary.size(), false); - BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { - assert(factor_->key1.type == NODE_POSE_3D); - //assert(factor_->key2.type == NODE_LANDMARK_3D); - const size_t& key1 = factor_->key1.index; - const size_t& key2 = factor_->key2.index; - if (dictionary[key1] == -1 || dictionary[key2] == -1) - continue; + // summarize the constraint number + const vector& dictionary = workspace.dictionary; + vector isValidCamera(workspace.dictionary.size(), false); + vector isValidLandmark(workspace.dictionary.size(), false); + BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { + assert(factor_->key1.type == NODE_POSE_3D); + //assert(factor_->key2.type == NODE_LANDMARK_3D); + const size_t& key1 = factor_->key1.index; + const size_t& key2 = factor_->key2.index; + if (dictionary[key1] == -1 || dictionary[key2] == -1) + continue; - isValidCamera[key1] = true; - if(factor_->key2.type == NODE_LANDMARK_3D) - isValidLandmark[key2] = true; - else - isValidCamera[key2] = true; + isValidCamera[key1] = true; + if(factor_->key2.type == NODE_LANDMARK_3D) + isValidLandmark[key2] = true; + else + isValidCamera[key2] = true; - nrConstraints[key1]++; - nrConstraints[key2]++; + nrConstraints[key1]++; + nrConstraints[key2]++; - // a single pose constraint is sufficient for stereo, so we add 2 to the counter - // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera - if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){ - nrConstraints[key1]+=2; - nrConstraints[key2]+=2; - } - } + // a single pose constraint is sufficient for stereo, so we add 2 to the counter + // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera + if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){ + nrConstraints[key1]+=2; + nrConstraints[key2]+=2; + } + } - // find the minimum constraint for cameras and landmarks - size_t minFoundConstraintsPerCamera = 10000; - size_t minFoundConstraintsPerLandmark = 10000; + // find the minimum constraint for cameras and landmarks + size_t minFoundConstraintsPerCamera = 10000; + size_t minFoundConstraintsPerLandmark = 10000; - for (size_t i=0; i(minFoundConstraintsPerCamera)); - if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark) - throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast(minFoundConstraintsPerLandmark)); - } + if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera) + throw runtime_error("checkSingularity:minConstraintsPerCamera < " + boost::lexical_cast(minFoundConstraintsPerCamera)); + if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark) + throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast(minFoundConstraintsPerLandmark)); + } }} // namespace diff --git a/gtsam_unstable/partition/GenericGraph.h b/gtsam_unstable/partition/GenericGraph.h index c722721b7..803a79719 100644 --- a/gtsam_unstable/partition/GenericGraph.h +++ b/gtsam_unstable/partition/GenericGraph.h @@ -17,133 +17,133 @@ namespace gtsam { namespace partition { - /*************************************************** - * 2D generic factors and their factor graph - ***************************************************/ - enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D }; + /*************************************************** + * 2D generic factors and their factor graph + ***************************************************/ + enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D }; - /** the index of the node and the type of the node */ - struct GenericNode2D { - std::size_t index; - GenericNode2DType type; - GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {} - }; + /** the index of the node and the type of the node */ + struct GenericNode2D { + std::size_t index; + GenericNode2DType type; + GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {} + }; - /** a factor always involves two nodes/variables for now */ - struct GenericFactor2D { - GenericNode2D key1; - GenericNode2D key2; - int index; // the factor index in the original nonlinear factor graph - int weight; // the weight of the edge - GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_ = -1, const int weight_ = 1) - : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {} - GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_ = -1, const int weight_ = 1) - : key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), - key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {} - }; + /** a factor always involves two nodes/variables for now */ + struct GenericFactor2D { + GenericNode2D key1; + GenericNode2D key2; + int index; // the factor index in the original nonlinear factor graph + int weight; // the weight of the edge + GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_ = -1, const int weight_ = 1) + : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {} + GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_ = -1, const int weight_ = 1) + : key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), + key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {} + }; - /** graph is a collection of factors */ - typedef boost::shared_ptr sharedGenericFactor2D; - typedef std::vector GenericGraph2D; + /** graph is a collection of factors */ + typedef boost::shared_ptr sharedGenericFactor2D; + typedef std::vector GenericGraph2D; - /** merge nodes in DSF using constraints captured by the given graph */ - std::list > findIslands(const GenericGraph2D& graph, const std::vector& keys, WorkSpace& workspace, - const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); + /** merge nodes in DSF using constraints captured by the given graph */ + std::list > findIslands(const GenericGraph2D& graph, const std::vector& keys, WorkSpace& workspace, + const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); - /** eliminate the sensors from generic graph */ - inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, - const std::vector& dictionary, GenericGraph2D& reducedGraph) { - throw std::runtime_error("reduceGenericGraph 2d not implemented"); - } + /** eliminate the sensors from generic graph */ + inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, + const std::vector& dictionary, GenericGraph2D& reducedGraph) { + throw std::runtime_error("reduceGenericGraph 2d not implemented"); + } - /** check whether the 2D graph is singular (under constrained) , Dummy function for 2D */ - inline void checkSingularity(const GenericGraph2D& graph, const std::vector& frontals, - WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; } + /** check whether the 2D graph is singular (under constrained) , Dummy function for 2D */ + inline void checkSingularity(const GenericGraph2D& graph, const std::vector& frontals, + WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; } - /** print the graph **/ - void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D"); + /** print the graph **/ + void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D"); - /*************************************************** - * 3D generic factors and their factor graph - ***************************************************/ - enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D }; + /*************************************************** + * 3D generic factors and their factor graph + ***************************************************/ + enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D }; -// const int minNrConstraintsPerCamera = 7; -// const int minNrConstraintsPerLandmark = 2; +// const int minNrConstraintsPerCamera = 7; +// const int minNrConstraintsPerLandmark = 2; - /** the index of the node and the type of the node */ - struct GenericNode3D { - std::size_t index; - GenericNode3DType type; - GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {} - }; + /** the index of the node and the type of the node */ + struct GenericNode3D { + std::size_t index; + GenericNode3DType type; + GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {} + }; - /** a factor always involves two nodes/variables for now */ - struct GenericFactor3D { - GenericNode3D key1; - GenericNode3D key2; - int index; // the index in the entire graph, 0-based - int weight; // the weight of the edge - GenericFactor3D() :key1(-1, NODE_POSE_3D), key2(-1, NODE_LANDMARK_3D), index(-1), weight(1) {} - GenericFactor3D(const size_t index1, const size_t index2, const int index_ = -1, - const GenericNode3DType type1 = NODE_POSE_3D, const GenericNode3DType type2 = NODE_LANDMARK_3D, const int weight_ = 1) - : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {} - }; + /** a factor always involves two nodes/variables for now */ + struct GenericFactor3D { + GenericNode3D key1; + GenericNode3D key2; + int index; // the index in the entire graph, 0-based + int weight; // the weight of the edge + GenericFactor3D() :key1(-1, NODE_POSE_3D), key2(-1, NODE_LANDMARK_3D), index(-1), weight(1) {} + GenericFactor3D(const size_t index1, const size_t index2, const int index_ = -1, + const GenericNode3DType type1 = NODE_POSE_3D, const GenericNode3DType type2 = NODE_LANDMARK_3D, const int weight_ = 1) + : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {} + }; - /** graph is a collection of factors */ - typedef boost::shared_ptr sharedGenericFactor3D; - typedef std::vector GenericGraph3D; + /** graph is a collection of factors */ + typedef boost::shared_ptr sharedGenericFactor3D; + typedef std::vector GenericGraph3D; - /** merge nodes in DSF using constraints captured by the given graph */ - std::list > findIslands(const GenericGraph3D& graph, const std::vector& keys, WorkSpace& workspace, - const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark); + /** merge nodes in DSF using constraints captured by the given graph */ + std::list > findIslands(const GenericGraph3D& graph, const std::vector& keys, WorkSpace& workspace, + const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark); - /** eliminate the sensors from generic graph */ - void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, - const std::vector& dictionary, GenericGraph3D& reducedGraph); + /** eliminate the sensors from generic graph */ + void reduceGenericGraph(const GenericGraph3D& graph, const std::vector& cameraKeys, const std::vector& landmarkKeys, + const std::vector& dictionary, GenericGraph3D& reducedGraph); - /** check whether the 3D graph is singular (under constrained) */ - void checkSingularity(const GenericGraph3D& graph, const std::vector& frontals, - WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark); + /** check whether the 3D graph is singular (under constrained) */ + void checkSingularity(const GenericGraph3D& graph, const std::vector& frontals, + WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark); - /** print the graph **/ - void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D"); + /** print the graph **/ + void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D"); - /*************************************************** - * unary generic factors and their factor graph - ***************************************************/ - /** a factor involves a single variable */ - struct GenericUnaryFactor { - GenericNode2D key; - int index; // the factor index in the original nonlinear factor graph - GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1) - : key(key_, type_), index(index_) {} - GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1) - : key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {} - }; + /*************************************************** + * unary generic factors and their factor graph + ***************************************************/ + /** a factor involves a single variable */ + struct GenericUnaryFactor { + GenericNode2D key; + int index; // the factor index in the original nonlinear factor graph + GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1) + : key(key_, type_), index(index_) {} + GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1) + : key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {} + }; - /** graph is a collection of factors */ - typedef boost::shared_ptr sharedGenericUnaryFactor; - typedef std::vector GenericUnaryGraph; + /** graph is a collection of factors */ + typedef boost::shared_ptr sharedGenericUnaryFactor; + typedef std::vector GenericUnaryGraph; - /*************************************************** - * utility functions - ***************************************************/ - inline bool hasCommonCamera(const std::set& cameras1, const std::set& cameras2) { - if (cameras1.empty() || cameras2.empty()) - throw std::invalid_argument("hasCommonCamera: the input camera set is empty!"); - std::set::const_iterator it1 = cameras1.begin(); - std::set::const_iterator it2 = cameras2.begin(); - while (it1 != cameras1.end() && it2 != cameras2.end()) { - if (*it1 == *it2) - return true; - else if (*it1 < *it2) - it1++; - else - it2++; - } - return false; - } + /*************************************************** + * utility functions + ***************************************************/ + inline bool hasCommonCamera(const std::set& cameras1, const std::set& cameras2) { + if (cameras1.empty() || cameras2.empty()) + throw std::invalid_argument("hasCommonCamera: the input camera set is empty!"); + std::set::const_iterator it1 = cameras1.begin(); + std::set::const_iterator it2 = cameras2.begin(); + while (it1 != cameras1.end() && it2 != cameras2.end()) { + if (*it1 == *it2) + return true; + else if (*it1 < *it2) + it1++; + else + it2++; + } + return false; + } }} // namespace diff --git a/gtsam_unstable/partition/NestedDissection-inl.h b/gtsam_unstable/partition/NestedDissection-inl.h index d6dafce49..6f1818a3e 100644 --- a/gtsam_unstable/partition/NestedDissection-inl.h +++ b/gtsam_unstable/partition/NestedDissection-inl.h @@ -16,236 +16,236 @@ namespace gtsam { namespace partition { - /* ************************************************************************* */ - template - NestedDissection::NestedDissection( - const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) : - fg_(fg), ordering_(ordering){ - GenericUnaryGraph unaryFactors; - GenericGraph gfg; - boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); + /* ************************************************************************* */ + template + NestedDissection::NestedDissection( + const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) : + fg_(fg), ordering_(ordering){ + GenericUnaryGraph unaryFactors; + GenericGraph gfg; + boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); - // build reverse mapping from integer to symbol - int numNodes = ordering.size(); - int2symbol_.resize(numNodes); - Ordering::const_iterator it = ordering.begin(), itLast = ordering.end(); - while(it != itLast) - int2symbol_[it->second] = (it++)->first; + // build reverse mapping from integer to symbol + int numNodes = ordering.size(); + int2symbol_.resize(numNodes); + Ordering::const_iterator it = ordering.begin(), itLast = ordering.end(); + while(it != itLast) + int2symbol_[it->second] = (it++)->first; - vector keys; - keys.reserve(numNodes); - for(int i=0; i keys; + keys.reserve(numNodes); + for(int i=0; i(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr(), workspace, verbose); - } + WorkSpace workspace(numNodes); + root_ = recursivePartition(gfg, unaryFactors, keys, vector(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr(), workspace, verbose); + } - /* ************************************************************************* */ - template - NestedDissection::NestedDissection( - const NLG& fg, const Ordering& ordering, const boost::shared_ptr& cuts, const bool verbose) : fg_(fg), ordering_(ordering){ - GenericUnaryGraph unaryFactors; - GenericGraph gfg; - boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); + /* ************************************************************************* */ + template + NestedDissection::NestedDissection( + const NLG& fg, const Ordering& ordering, const boost::shared_ptr& cuts, const bool verbose) : fg_(fg), ordering_(ordering){ + GenericUnaryGraph unaryFactors; + GenericGraph gfg; + boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); - // build reverse mapping from integer to symbol - int numNodes = ordering.size(); - int2symbol_.resize(numNodes); - Ordering::const_iterator it = ordering.begin(), itLast = ordering.end(); - while(it != itLast) - int2symbol_[it->second] = (it++)->first; + // build reverse mapping from integer to symbol + int numNodes = ordering.size(); + int2symbol_.resize(numNodes); + Ordering::const_iterator it = ordering.begin(), itLast = ordering.end(); + while(it != itLast) + int2symbol_[it->second] = (it++)->first; - vector keys; - keys.reserve(numNodes); - for(int i=0; i keys; + keys.reserve(numNodes); + for(int i=0; i(), cuts, boost::shared_ptr(), workspace, verbose); - } + WorkSpace workspace(numNodes); + root_ = recursivePartition(gfg, unaryFactors, keys, vector(), cuts, boost::shared_ptr(), workspace, verbose); + } - /* ************************************************************************* */ - template - boost::shared_ptr NestedDissection::makeSubNLG( - const NLG& fg, const vector& frontals, const vector& sep, const boost::shared_ptr& parent) const { - OrderedSymbols frontalKeys; - BOOST_FOREACH(const size_t index, frontals) - frontalKeys.push_back(int2symbol_[index]); + /* ************************************************************************* */ + template + boost::shared_ptr NestedDissection::makeSubNLG( + const NLG& fg, const vector& frontals, const vector& sep, const boost::shared_ptr& parent) const { + OrderedSymbols frontalKeys; + BOOST_FOREACH(const size_t index, frontals) + frontalKeys.push_back(int2symbol_[index]); - UnorderedSymbols sepKeys; - BOOST_FOREACH(const size_t index, sep) - sepKeys.insert(int2symbol_[index]); + UnorderedSymbols sepKeys; + BOOST_FOREACH(const size_t index, sep) + sepKeys.insert(int2symbol_[index]); - return boost::make_shared(fg, frontalKeys, sepKeys, parent); - } + return boost::make_shared(fg, frontalKeys, sepKeys, parent); + } - /* ************************************************************************* */ - template - void NestedDissection::processFactor( - const typename GenericGraph::value_type& factor, const std::vector& partitionTable, // input - vector& frontalFactors, NLG& sepFactors, vector >& childSeps, // output factor graphs - typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques - list sep_; // the separator variables involved in the current factor - int partition1 = partitionTable[factor->key1.index]; - int partition2 = partitionTable[factor->key2.index]; - if (partition1 <= 0 && partition2 <= 0) { // is a factor in the current clique - sepFactors.push_back(fg_[factor->index]); - } - else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) { // is a weeklink (factor between two child cliques) - weeklinks.push_back(fg_[factor->index]); - } - else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques - frontalFactors[partition1 - 1].push_back(factor); - } - else { // is a joint factor in the child clique (involving varaibles in the current clique) - if (partition1 > 0 && partition2 <= 0) { - frontalFactors[partition1 - 1].push_back(factor); - childSeps[partition1 - 1].insert(factor->key2.index); - } else if (partition1 <= 0 && partition2 > 0) { - frontalFactors[partition2 - 1].push_back(factor); - childSeps[partition2 - 1].insert(factor->key1.index); - } else - throw runtime_error("processFactor: unexpected entries in the partition table!"); - } - } + /* ************************************************************************* */ + template + void NestedDissection::processFactor( + const typename GenericGraph::value_type& factor, const std::vector& partitionTable, // input + vector& frontalFactors, NLG& sepFactors, vector >& childSeps, // output factor graphs + typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques + list sep_; // the separator variables involved in the current factor + int partition1 = partitionTable[factor->key1.index]; + int partition2 = partitionTable[factor->key2.index]; + if (partition1 <= 0 && partition2 <= 0) { // is a factor in the current clique + sepFactors.push_back(fg_[factor->index]); + } + else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) { // is a weeklink (factor between two child cliques) + weeklinks.push_back(fg_[factor->index]); + } + else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques + frontalFactors[partition1 - 1].push_back(factor); + } + else { // is a joint factor in the child clique (involving varaibles in the current clique) + if (partition1 > 0 && partition2 <= 0) { + frontalFactors[partition1 - 1].push_back(factor); + childSeps[partition1 - 1].insert(factor->key2.index); + } else if (partition1 <= 0 && partition2 > 0) { + frontalFactors[partition2 - 1].push_back(factor); + childSeps[partition2 - 1].insert(factor->key1.index); + } else + throw runtime_error("processFactor: unexpected entries in the partition table!"); + } + } - /* ************************************************************************* */ - /** - * given a factor graph and its partition {nodeMap}, split the factors between the child cliques ({frontalFactors}) - * and the current clique ({sepFactors}). Also split the variables between the child cliques ({childFrontals}) - * and the current clique ({localFrontals}). Those separator variables involved in {frontalFactors} are put into - * the correspoding ordering in {childSeps}. - */ - // TODO: frontalFactors and localFrontals should be generated in findSeparator - template - void NestedDissection::partitionFactorsAndVariables( - const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector& keys, //input - const std::vector& partitionTable, const int numSubmaps, // input - vector& frontalFactors, vector& frontalUnaryFactors, NLG& sepFactors, // output factor graphs - vector >& childFrontals, vector >& childSeps, vector& localFrontals, // output sub-orderings - typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques + /* ************************************************************************* */ + /** + * given a factor graph and its partition {nodeMap}, split the factors between the child cliques ({frontalFactors}) + * and the current clique ({sepFactors}). Also split the variables between the child cliques ({childFrontals}) + * and the current clique ({localFrontals}). Those separator variables involved in {frontalFactors} are put into + * the correspoding ordering in {childSeps}. + */ + // TODO: frontalFactors and localFrontals should be generated in findSeparator + template + void NestedDissection::partitionFactorsAndVariables( + const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector& keys, //input + const std::vector& partitionTable, const int numSubmaps, // input + vector& frontalFactors, vector& frontalUnaryFactors, NLG& sepFactors, // output factor graphs + vector >& childFrontals, vector >& childSeps, vector& localFrontals, // output sub-orderings + typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques - // make three lists of variables A, B, and C - int partition; - childFrontals.resize(numSubmaps); - BOOST_FOREACH(const size_t key, keys){ - partition = partitionTable[key]; - switch (partition) { - case -1: break; // the separator of the separator variables - case 0: localFrontals.push_back(key); break; // the separator variables - default: childFrontals[partition-1].push_back(key); // the frontal variables - } - } + // make three lists of variables A, B, and C + int partition; + childFrontals.resize(numSubmaps); + BOOST_FOREACH(const size_t key, keys){ + partition = partitionTable[key]; + switch (partition) { + case -1: break; // the separator of the separator variables + case 0: localFrontals.push_back(key); break; // the separator variables + default: childFrontals[partition-1].push_back(key); // the frontal variables + } + } - // group the factors to {frontalFactors} and {sepFactors},and find the joint variables - vector > childSeps_; - childSeps_.resize(numSubmaps); - childSeps.reserve(numSubmaps); - frontalFactors.resize(numSubmaps); - frontalUnaryFactors.resize(numSubmaps); - BOOST_FOREACH(typename GenericGraph::value_type factor, fg) - processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks); - BOOST_FOREACH(const set& childSep, childSeps_) - childSeps.push_back(vector(childSep.begin(), childSep.end())); + // group the factors to {frontalFactors} and {sepFactors},and find the joint variables + vector > childSeps_; + childSeps_.resize(numSubmaps); + childSeps.reserve(numSubmaps); + frontalFactors.resize(numSubmaps); + frontalUnaryFactors.resize(numSubmaps); + BOOST_FOREACH(typename GenericGraph::value_type factor, fg) + processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks); + BOOST_FOREACH(const set& childSep, childSeps_) + childSeps.push_back(vector(childSep.begin(), childSep.end())); - // add unary factor to the current cluster or pass it to one of the child clusters - BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) { - partition = partitionTable[unaryFactor_->key.index]; - if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]); - else frontalUnaryFactors[partition-1].push_back(unaryFactor_); - } - } + // add unary factor to the current cluster or pass it to one of the child clusters + BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) { + partition = partitionTable[unaryFactor_->key.index]; + if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]); + else frontalUnaryFactors[partition-1].push_back(unaryFactor_); + } + } - /* ************************************************************************* */ - template - NLG NestedDissection::collectOriginalFactors( - const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const { - NLG sepFactors; - typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end(); - while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]); - BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) - sepFactors.push_back(fg_[unaryFactor_->index]); - return sepFactors; - } + /* ************************************************************************* */ + template + NLG NestedDissection::collectOriginalFactors( + const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const { + NLG sepFactors; + typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end(); + while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]); + BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) + sepFactors.push_back(fg_[unaryFactor_->index]); + return sepFactors; + } - /* ************************************************************************* */ - template - boost::shared_ptr NestedDissection::recursivePartition( - const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector& frontals, const vector& sep, - const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { + /* ************************************************************************* */ + template + boost::shared_ptr NestedDissection::recursivePartition( + const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector& frontals, const vector& sep, + const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { - // if no split needed - NLG sepFactors; // factors that should remain in the current cluster - if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) { - sepFactors = collectOriginalFactors(gfg, unaryFactors); - return makeSubNLG(sepFactors, frontals, sep, parent); - } + // if no split needed + NLG sepFactors; // factors that should remain in the current cluster + if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) { + sepFactors = collectOriginalFactors(gfg, unaryFactors); + return makeSubNLG(sepFactors, frontals, sep, parent); + } - // find the nested dissection separator - int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(), - NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark()); - partition::PartitionTable& partitionTable = workspace.partitionTable; - if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!"); + // find the nested dissection separator + int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(), + NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark()); + partition::PartitionTable& partitionTable = workspace.partitionTable; + if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!"); - // split the factors between child cliques and the current clique - vector frontalFactors; vector frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks; - vector localFrontals; vector > childFrontals, childSeps; - partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps, - frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); + // split the factors between child cliques and the current clique + vector frontalFactors; vector frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks; + vector localFrontals; vector > childFrontals, childSeps; + partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps, + frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); - // make a new cluster - boost::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); - current->setWeeklinks(weeklinks); + // make a new cluster + boost::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); + current->setWeeklinks(weeklinks); - // check whether all the submaps are fully constrained - for (int i=0; i child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], - numNodeStopPartition, minNodesPerMap, current, workspace, verbose); - current->addChild(child); - } + // create child clusters + for (int i=0; i child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], + numNodeStopPartition, minNodesPerMap, current, workspace, verbose); + current->addChild(child); + } - return current; - } + return current; + } - /* ************************************************************************* */ - template - boost::shared_ptr NestedDissection::recursivePartition( - const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector& frontals, const vector& sep, - const boost::shared_ptr& cuts, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { + /* ************************************************************************* */ + template + boost::shared_ptr NestedDissection::recursivePartition( + const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector& frontals, const vector& sep, + const boost::shared_ptr& cuts, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const { - // if there is no need to cut any more - NLG sepFactors; // factors that should remain in the current cluster - if (!cuts.get()) { - sepFactors = collectOriginalFactors(gfg, unaryFactors); - return makeSubNLG(sepFactors, frontals, sep, parent); - } + // if there is no need to cut any more + NLG sepFactors; // factors that should remain in the current cluster + if (!cuts.get()) { + sepFactors = collectOriginalFactors(gfg, unaryFactors); + return makeSubNLG(sepFactors, frontals, sep, parent); + } - // retrieve the current partitioning info - int numSubmaps = 2; - partition::PartitionTable& partitionTable = cuts->partitionTable; + // retrieve the current partitioning info + int numSubmaps = 2; + partition::PartitionTable& partitionTable = cuts->partitionTable; - // split the factors between child cliques and the current clique - vector frontalFactors; vector frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks; - vector localFrontals; vector > childFrontals, childSeps; - partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps, - frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); + // split the factors between child cliques and the current clique + vector frontalFactors; vector frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks; + vector localFrontals; vector > childFrontals, childSeps; + partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps, + frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); - // make a new cluster - boost::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); - current->setWeeklinks(weeklinks); + // make a new cluster + boost::shared_ptr current = makeSubNLG(sepFactors, localFrontals, sep, parent); + current->setWeeklinks(weeklinks); - // create child clusters - for (int i=0; i<2; i++) { - boost::shared_ptr child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], - cuts->children.empty() ? boost::shared_ptr() : cuts->children[i], current, workspace, verbose); - current->addChild(child); - } - return current; - } + // create child clusters + for (int i=0; i<2; i++) { + boost::shared_ptr child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], + cuts->children.empty() ? boost::shared_ptr() : cuts->children[i], current, workspace, verbose); + current->addChild(child); + } + return current; + } }} //namespace diff --git a/gtsam_unstable/partition/NestedDissection.h b/gtsam_unstable/partition/NestedDissection.h index 3c294be64..82304a79d 100644 --- a/gtsam_unstable/partition/NestedDissection.h +++ b/gtsam_unstable/partition/NestedDissection.h @@ -14,56 +14,56 @@ namespace gtsam { namespace partition { - /** - * Apply nested dissection algorithm to nonlinear factor graphs - */ - template - class NestedDissection { - public: - typedef boost::shared_ptr sharedSubNLG; + /** + * Apply nested dissection algorithm to nonlinear factor graphs + */ + template + class NestedDissection { + public: + typedef boost::shared_ptr sharedSubNLG; - private: - NLG fg_; // the original nonlinear factor graph - Ordering ordering_; // the variable ordering in the nonlinear factor graph - std::vector int2symbol_; // the mapping from integer key to symbol - sharedSubNLG root_; // the root of generated cluster tree + private: + NLG fg_; // the original nonlinear factor graph + Ordering ordering_; // the variable ordering in the nonlinear factor graph + std::vector int2symbol_; // the mapping from integer key to symbol + sharedSubNLG root_; // the root of generated cluster tree - public: - sharedSubNLG root() const { return root_; } + public: + sharedSubNLG root() const { return root_; } - public: - /* constructor with post-determined partitoning*/ - NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false); + public: + /* constructor with post-determined partitoning*/ + NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false); - /* constructor with pre-determined cuts*/ - NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr& cuts, const bool verbose = false); + /* constructor with pre-determined cuts*/ + NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr& cuts, const bool verbose = false); - private: - /* convert generic subgraph to nonlinear subgraph */ - sharedSubNLG makeSubNLG(const NLG& fg, const std::vector& frontals, const std::vector& sep, const boost::shared_ptr& parent) const; + private: + /* convert generic subgraph to nonlinear subgraph */ + sharedSubNLG makeSubNLG(const NLG& fg, const std::vector& frontals, const std::vector& sep, const boost::shared_ptr& parent) const; - void processFactor(const typename GenericGraph::value_type& factor, const std::vector& partitionTable, // input - std::vector& frontalFactors, NLG& sepFactors, std::vector >& childSeps, // output factor graphs - typename SubNLG::Weeklinks& weeklinks) const; + void processFactor(const typename GenericGraph::value_type& factor, const std::vector& partitionTable, // input + std::vector& frontalFactors, NLG& sepFactors, std::vector >& childSeps, // output factor graphs + typename SubNLG::Weeklinks& weeklinks) const; - /* recursively partition the generic graph */ - void partitionFactorsAndVariables( - const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, - const std::vector& keys, const std::vector& partitionTable, const int numSubmaps, // input - std::vector& frontalFactors, vector& frontalUnaryFactors, NLG& sepFactors, // output factor graphs - std::vector >& childFrontals, std::vector >& childSeps, std::vector& localFrontals, // output sub-orderings - typename SubNLG::Weeklinks& weeklinks) const; + /* recursively partition the generic graph */ + void partitionFactorsAndVariables( + const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, + const std::vector& keys, const std::vector& partitionTable, const int numSubmaps, // input + std::vector& frontalFactors, vector& frontalUnaryFactors, NLG& sepFactors, // output factor graphs + std::vector >& childFrontals, std::vector >& childSeps, std::vector& localFrontals, // output sub-orderings + typename SubNLG::Weeklinks& weeklinks) const; - NLG collectOriginalFactors(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const; + NLG collectOriginalFactors(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const; - /* recursively partition the generic graph */ - sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector& frontals, const std::vector& sep, - const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; + /* recursively partition the generic graph */ + sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector& frontals, const std::vector& sep, + const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; - /* recursively partition the generic graph */ - sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector& frontals, const std::vector& sep, - const boost::shared_ptr& cuts, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; + /* recursively partition the generic graph */ + sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector& frontals, const std::vector& sep, + const boost::shared_ptr& cuts, const boost::shared_ptr& parent, WorkSpace& workspace, const bool verbose) const; - }; + }; }} //namespace diff --git a/gtsam_unstable/partition/PartitionWorkSpace.h b/gtsam_unstable/partition/PartitionWorkSpace.h index b268c0fb2..b0bd8113e 100644 --- a/gtsam_unstable/partition/PartitionWorkSpace.h +++ b/gtsam_unstable/partition/PartitionWorkSpace.h @@ -13,32 +13,32 @@ namespace gtsam { namespace partition { - typedef std::vector PartitionTable; + typedef std::vector PartitionTable; - // the work space, preallocated memory - struct WorkSpace { - std::vector dictionary; // a mapping from the integer key in the original graph to 0-based index in the subgraph, useful when handling a subset of keys and graphs - boost::shared_ptr > dsf; // a block memory pre-allocated for DSFVector - PartitionTable partitionTable; // a mapping from a key to the submap index, 0 means the separator, i means the ith submap + // the work space, preallocated memory + struct WorkSpace { + std::vector dictionary; // a mapping from the integer key in the original graph to 0-based index in the subgraph, useful when handling a subset of keys and graphs + boost::shared_ptr > dsf; // a block memory pre-allocated for DSFVector + PartitionTable partitionTable; // a mapping from a key to the submap index, 0 means the separator, i means the ith submap - // constructor - WorkSpace(const size_t numNodes) : dictionary(numNodes,0), - dsf(new std::vector(numNodes, 0)), partitionTable(numNodes, -1) { } + // constructor + WorkSpace(const size_t numNodes) : dictionary(numNodes,0), + dsf(new std::vector(numNodes, 0)), partitionTable(numNodes, -1) { } - // set up dictionary: -1: no such key, none-zero: the corresponding 0-based index - inline void prepareDictionary(const std::vector& keys) { - int index = 0; - std::fill(dictionary.begin(), dictionary.end(), -1); - std::vector::const_iterator it=keys.begin(), itLast=keys.end(); - while(it!=itLast) dictionary[*(it++)] = index++; - } - }; + // set up dictionary: -1: no such key, none-zero: the corresponding 0-based index + inline void prepareDictionary(const std::vector& keys) { + int index = 0; + std::fill(dictionary.begin(), dictionary.end(), -1); + std::vector::const_iterator it=keys.begin(), itLast=keys.end(); + while(it!=itLast) dictionary[*(it++)] = index++; + } + }; - // manually defined cuts - struct Cuts { - PartitionTable partitionTable; - std::vector > children; - }; + // manually defined cuts + struct Cuts { + PartitionTable partitionTable; + std::vector > children; + }; }} // 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/partition/tests/testGenericGraph.cpp b/gtsam_unstable/partition/tests/testGenericGraph.cpp index 7d9bf928f..c6e432902 100644 --- a/gtsam_unstable/partition/tests/testGenericGraph.cpp +++ b/gtsam_unstable/partition/tests/testGenericGraph.cpp @@ -29,29 +29,29 @@ using namespace gtsam::partition; */ TEST ( GenerciGraph, findIslands ) { - GenericGraph2D graph; - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); - graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); - std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9; - WorkSpace workspace(10); // from 0 to 9 - list > islands = findIslands(graph, keys, workspace, 7, 2); - LONGS_EQUAL(2, islands.size()); - vector island1; island1 += 1, 2, 3, 7, 8; - vector island2; island2 += 4, 5, 6, 9; - CHECK(island1 == islands.front()); - CHECK(island2 == islands.back()); + WorkSpace workspace(10); // from 0 to 9 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 2, 3, 7, 8; + vector island2; island2 += 4, 5, 6, 9; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); } /* ************************************************************************* */ @@ -62,27 +62,27 @@ TEST ( GenerciGraph, findIslands ) */ TEST( GenerciGraph, findIslands2 ) { - GenericGraph2D graph; - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); - graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); - std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 2, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_POSE_2D)); + graph.push_back(boost::make_shared(5, NODE_POSE_2D, 6, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6, 7, 8; - WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here - list > islands = findIslands(graph, keys, workspace, 7, 2); - LONGS_EQUAL(1, islands.size()); - vector island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8; - CHECK(island1 == islands.front()); + WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(1, islands.size()); + vector island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8; + CHECK(island1 == islands.front()); } /* ************************************************************************* */ @@ -92,21 +92,21 @@ TEST( GenerciGraph, findIslands2 ) */ TEST ( GenerciGraph, findIslands3 ) { - GenericGraph2D graph; - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_POSE_2D)); - std::vector keys; keys += 1, 2, 3, 4, 5, 6; + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_POSE_2D)); + std::vector keys; keys += 1, 2, 3, 4, 5, 6; - WorkSpace workspace(7); // from 0 to 9 - list > islands = findIslands(graph, keys, workspace, 7, 2); - LONGS_EQUAL(2, islands.size()); - vector island1; island1 += 1, 5; - vector island2; island2 += 2, 3, 4, 6; - CHECK(island1 == islands.front()); - CHECK(island2 == islands.back()); + WorkSpace workspace(7); // from 0 to 9 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 5; + vector island2; island2 += 2, 3, 4, 6; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); } /* ************************************************************************* */ @@ -115,18 +115,18 @@ TEST ( GenerciGraph, findIslands3 ) */ TEST ( GenerciGraph, findIslands4 ) { - GenericGraph2D graph; - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); - std::vector keys; keys += 3, 4, 7; + GenericGraph2D graph; + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); + std::vector keys; keys += 3, 4, 7; - WorkSpace workspace(8); // from 0 to 7 - list > islands = findIslands(graph, keys, workspace, 7, 2); - LONGS_EQUAL(2, islands.size()); - vector island1; island1 += 3, 4; - vector island2; island2 += 7; - CHECK(island1 == islands.front()); - CHECK(island2 == islands.back()); + WorkSpace workspace(8); // from 0 to 7 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 3, 4; + vector island2; island2 += 7; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); } /* ************************************************************************* */ @@ -137,24 +137,24 @@ TEST ( GenerciGraph, findIslands4 ) */ TEST ( GenerciGraph, findIslands5 ) { - GenericGraph2D graph; - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + GenericGraph2D graph; + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); + graph.push_back(boost::make_shared(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); - graph.push_back(boost::make_shared(1, NODE_POSE_2D, 3, NODE_POSE_2D)); - graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_POSE_2D)); + graph.push_back(boost::make_shared(1, NODE_POSE_2D, 3, NODE_POSE_2D)); + graph.push_back(boost::make_shared(2, NODE_POSE_2D, 4, NODE_POSE_2D)); - std::vector keys; keys += 1, 2, 3, 4, 5; + std::vector keys; keys += 1, 2, 3, 4, 5; - WorkSpace workspace(6); // from 0 to 5 - list > islands = findIslands(graph, keys, workspace, 7, 2); - LONGS_EQUAL(2, islands.size()); - vector island1; island1 += 1, 3, 5; - vector island2; island2 += 2, 4; - CHECK(island1 == islands.front()); - CHECK(island2 == islands.back()); + WorkSpace workspace(6); // from 0 to 5 + list > islands = findIslands(graph, keys, workspace, 7, 2); + LONGS_EQUAL(2, islands.size()); + vector island1; island1 += 1, 3, 5; + vector island2; island2 += 2, 4; + CHECK(island1 == islands.front()); + CHECK(island2 == islands.back()); } /* ************************************************************************* */ @@ -165,31 +165,31 @@ TEST ( GenerciGraph, findIslands5 ) */ TEST ( GenerciGraph, reduceGenericGraph ) { - GenericGraph3D graph; - graph.push_back(boost::make_shared(1, 3)); - graph.push_back(boost::make_shared(1, 4)); - graph.push_back(boost::make_shared(1, 5)); - graph.push_back(boost::make_shared(2, 5)); - graph.push_back(boost::make_shared(2, 6)); + GenericGraph3D graph; + graph.push_back(boost::make_shared(1, 3)); + graph.push_back(boost::make_shared(1, 4)); + graph.push_back(boost::make_shared(1, 5)); + graph.push_back(boost::make_shared(2, 5)); + graph.push_back(boost::make_shared(2, 6)); - std::vector cameraKeys, landmarkKeys; - cameraKeys.push_back(1); - cameraKeys.push_back(2); - landmarkKeys.push_back(3); - landmarkKeys.push_back(4); - landmarkKeys.push_back(5); - landmarkKeys.push_back(6); + std::vector cameraKeys, landmarkKeys; + cameraKeys.push_back(1); + cameraKeys.push_back(2); + landmarkKeys.push_back(3); + landmarkKeys.push_back(4); + landmarkKeys.push_back(5); + landmarkKeys.push_back(6); - std::vector dictionary; - dictionary.resize(7, -1); // from 0 to 6 - dictionary[1] = 0; - dictionary[2] = 1; + std::vector dictionary; + dictionary.resize(7, -1); // from 0 to 6 + dictionary[1] = 0; + dictionary[2] = 1; - GenericGraph3D reduced; - std::map > cameraToLandmarks; - reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); - LONGS_EQUAL(1, reduced.size()); - LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); + GenericGraph3D reduced; + std::map > cameraToLandmarks; + reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); + LONGS_EQUAL(1, reduced.size()); + LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); } /* ************************************************************************* */ @@ -200,53 +200,53 @@ TEST ( GenerciGraph, reduceGenericGraph ) */ TEST ( GenericGraph, reduceGenericGraph2 ) { - GenericGraph3D graph; - graph.push_back(boost::make_shared(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D)); - graph.push_back(boost::make_shared(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D)); + GenericGraph3D graph; + graph.push_back(boost::make_shared(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D)); + graph.push_back(boost::make_shared(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D)); - std::vector cameraKeys, landmarkKeys; - cameraKeys.push_back(1); - cameraKeys.push_back(2); - cameraKeys.push_back(7); - landmarkKeys.push_back(3); - landmarkKeys.push_back(4); - landmarkKeys.push_back(5); - landmarkKeys.push_back(6); + std::vector cameraKeys, landmarkKeys; + cameraKeys.push_back(1); + cameraKeys.push_back(2); + cameraKeys.push_back(7); + landmarkKeys.push_back(3); + landmarkKeys.push_back(4); + landmarkKeys.push_back(5); + landmarkKeys.push_back(6); - std::vector dictionary; - dictionary.resize(8, -1); // from 0 to 7 - dictionary[1] = 0; - dictionary[2] = 1; - dictionary[7] = 6; + std::vector dictionary; + dictionary.resize(8, -1); // from 0 to 7 + dictionary[1] = 0; + dictionary[2] = 1; + dictionary[7] = 6; - GenericGraph3D reduced; - std::map > cameraToLandmarks; - reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); - LONGS_EQUAL(2, reduced.size()); - LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); - LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index); + GenericGraph3D reduced; + std::map > cameraToLandmarks; + reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); + LONGS_EQUAL(2, reduced.size()); + LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); + LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index); } /* ************************************************************************* */ TEST ( GenerciGraph, hasCommonCamera ) { - std::set cameras1; cameras1 += 1, 2, 3, 4, 5; - std::set cameras2; cameras2 += 8, 7, 6, 5; - bool actual = hasCommonCamera(cameras1, cameras2); - CHECK(actual); + std::set cameras1; cameras1 += 1, 2, 3, 4, 5; + std::set cameras2; cameras2 += 8, 7, 6, 5; + bool actual = hasCommonCamera(cameras1, cameras2); + CHECK(actual); } /* ************************************************************************* */ TEST ( GenerciGraph, hasCommonCamera2 ) { - std::set cameras1; cameras1 += 1, 3, 5, 7; - std::set cameras2; cameras2 += 2, 4, 6, 8, 10; - bool actual = hasCommonCamera(cameras1, cameras2); - CHECK(!actual); + std::set cameras1; cameras1 += 1, 3, 5, 7; + std::set cameras2; cameras2 += 2, 4, 6, 8, 10; + bool actual = hasCommonCamera(cameras1, cameras2); + CHECK(!actual); } /* ************************************************************************* */ diff --git a/gtsam_unstable/partition/tests/testNestedDissection.cpp b/gtsam_unstable/partition/tests/testNestedDissection.cpp index 906178914..d7035c2d8 100644 --- a/gtsam_unstable/partition/tests/testNestedDissection.cpp +++ b/gtsam_unstable/partition/tests/testNestedDissection.cpp @@ -32,22 +32,22 @@ using namespace gtsam::partition; // l1 TEST ( NestedDissection, oneIsland ) { - using namespace submapPlanarSLAM; - typedef TSAM2D::SubNLG SubNLG; - Graph fg; - fg.addOdometry(1, 2, Pose2(), odoNoise); - fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise); - fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise); - fg.addPoseConstraint(1, Pose2()); + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise); + fg.addPoseConstraint(1, Pose2()); - Ordering ordering; ordering += x1, x2, l1; + Ordering ordering; ordering += x1, x2, l1; - int numNodeStopPartition = 1e3; - int minNodesPerMap = 1e3; - NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); - LONGS_EQUAL(4, nd.root()->size()); - LONGS_EQUAL(3, nd.root()->frontal().size()); - LONGS_EQUAL(0, nd.root()->children().size()); + int numNodeStopPartition = 1e3; + int minNodesPerMap = 1e3; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(4, nd.root()->size()); + LONGS_EQUAL(3, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->children().size()); } /* ************************************************************************* */ @@ -56,35 +56,35 @@ TEST ( NestedDissection, oneIsland ) // x2/ \x5 TEST ( NestedDissection, TwoIslands ) { - using namespace submapPlanarSLAM; - typedef TSAM2D::SubNLG SubNLG; - Graph fg; - fg.addOdometry(1, 2, Pose2(), odoNoise); - fg.addOdometry(1, 3, Pose2(), odoNoise); - fg.addOdometry(2, 3, Pose2(), odoNoise); - fg.addOdometry(3, 4, Pose2(), odoNoise); - fg.addOdometry(4, 5, Pose2(), odoNoise); - fg.addOdometry(3, 5, Pose2(), odoNoise); - fg.addPoseConstraint(1, Pose2()); - fg.addPoseConstraint(4, Pose2()); - Ordering ordering; ordering += x1, x2, x3, x4, x5; + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addOdometry(1, 3, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addOdometry(4, 5, Pose2(), odoNoise); + fg.addOdometry(3, 5, Pose2(), odoNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5; - int numNodeStopPartition = 2; - int minNodesPerMap = 1; - NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); - // root submap - LONGS_EQUAL(0, nd.root()->size()); - LONGS_EQUAL(1, nd.root()->frontal().size()); - LONGS_EQUAL(0, nd.root()->separator().size()); - LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + // root submap + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps - // the 1st submap - LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); - LONGS_EQUAL(4, nd.root()->children()[0]->size()); + // the 1st submap + LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); + LONGS_EQUAL(4, nd.root()->children()[0]->size()); - // the 2nd submap - LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); - LONGS_EQUAL(4, nd.root()->children()[1]->size()); + // the 2nd submap + LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); + LONGS_EQUAL(4, nd.root()->children()[1]->size()); } /* ************************************************************************* */ @@ -93,40 +93,40 @@ TEST ( NestedDissection, TwoIslands ) // x2/ \x5 TEST ( NestedDissection, FourIslands ) { - using namespace submapPlanarSLAM; - typedef TSAM2D::SubNLG SubNLG; - Graph fg; - fg.addOdometry(1, 3, Pose2(), odoNoise); - fg.addOdometry(2, 3, Pose2(), odoNoise); - fg.addOdometry(3, 4, Pose2(), odoNoise); - fg.addOdometry(3, 5, Pose2(), odoNoise); - fg.addPoseConstraint(1, Pose2()); - fg.addPoseConstraint(4, Pose2()); - Ordering ordering; ordering += x1, x2, x3, x4, x5; + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 3, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addOdometry(3, 5, Pose2(), odoNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5; - int numNodeStopPartition = 2; - int minNodesPerMap = 1; - NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); - LONGS_EQUAL(0, nd.root()->size()); - LONGS_EQUAL(1, nd.root()->frontal().size()); - LONGS_EQUAL(0, nd.root()->separator().size()); - LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps - // the 1st submap - LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); - LONGS_EQUAL(2, nd.root()->children()[0]->size()); + // the 1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); + LONGS_EQUAL(2, nd.root()->children()[0]->size()); - // the 2nd submap - LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); - LONGS_EQUAL(2, nd.root()->children()[1]->size()); + // the 2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); + LONGS_EQUAL(2, nd.root()->children()[1]->size()); - // the 3rd submap - LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); - LONGS_EQUAL(1, nd.root()->children()[2]->size()); + // the 3rd submap + LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); + LONGS_EQUAL(1, nd.root()->children()[2]->size()); - // the 4th submap - LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size()); - LONGS_EQUAL(1, nd.root()->children()[3]->size()); + // the 4th submap + LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size()); + LONGS_EQUAL(1, nd.root()->children()[3]->size()); } /* ************************************************************************* */ @@ -137,41 +137,41 @@ TEST ( NestedDissection, FourIslands ) // x5 TEST ( NestedDissection, weekLinks ) { - using namespace submapPlanarSLAM; - typedef TSAM2D::SubNLG SubNLG; - Graph fg; - fg.addOdometry(1, 2, Pose2(), odoNoise); - fg.addOdometry(2, 3, Pose2(), odoNoise); - fg.addOdometry(2, 4, Pose2(), odoNoise); - fg.addOdometry(3, 4, Pose2(), odoNoise); - fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise); - fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise); - fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise); - fg.addPoseConstraint(1, Pose2()); - fg.addPoseConstraint(4, Pose2()); - fg.addPoseConstraint(5, Pose2()); - Ordering ordering; ordering += x1, x2, x3, x4, x5, l6; + using namespace submapPlanarSLAM; + typedef TSAM2D::SubNLG SubNLG; + Graph fg; + fg.addOdometry(1, 2, Pose2(), odoNoise); + fg.addOdometry(2, 3, Pose2(), odoNoise); + fg.addOdometry(2, 4, Pose2(), odoNoise); + fg.addOdometry(3, 4, Pose2(), odoNoise); + fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise); + fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise); + fg.addPoseConstraint(1, Pose2()); + fg.addPoseConstraint(4, Pose2()); + fg.addPoseConstraint(5, Pose2()); + Ordering ordering; ordering += x1, x2, x3, x4, x5, l6; - int numNodeStopPartition = 2; - int minNodesPerMap = 1; - NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); - LONGS_EQUAL(0, nd.root()->size()); // one weeklink - LONGS_EQUAL(1, nd.root()->frontal().size()); - LONGS_EQUAL(0, nd.root()->separator().size()); - LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps - LONGS_EQUAL(1, nd.root()->weeklinks().size()); + int numNodeStopPartition = 2; + int minNodesPerMap = 1; + NestedDissection nd(fg, ordering, numNodeStopPartition, minNodesPerMap); + LONGS_EQUAL(0, nd.root()->size()); // one weeklink + LONGS_EQUAL(1, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps + LONGS_EQUAL(1, nd.root()->weeklinks().size()); - // the 1st submap - LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4 - LONGS_EQUAL(4, nd.root()->children()[0]->size()); + // the 1st submap + LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4 + LONGS_EQUAL(4, nd.root()->children()[0]->size()); - // the 2nd submap - LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6 - LONGS_EQUAL(4, nd.root()->children()[1]->size()); - // - // the 3rd submap - LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5 - LONGS_EQUAL(1, nd.root()->children()[2]->size()); + // the 2nd submap + LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6 + LONGS_EQUAL(4, nd.root()->children()[1]->size()); + // + // the 3rd submap + LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5 + LONGS_EQUAL(1, nd.root()->children()[2]->size()); } /* ************************************************************************* */ @@ -184,86 +184,86 @@ TEST ( NestedDissection, weekLinks ) */ TEST ( NestedDissection, manual_cuts ) { - using namespace submapPlanarSLAM; - typedef partition::Cuts Cuts; - typedef TSAM2D::SubNLG SubNLG; - typedef partition::PartitionTable PartitionTable; - Graph fg; - fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise); - fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise); + using namespace submapPlanarSLAM; + typedef partition::Cuts Cuts; + typedef TSAM2D::SubNLG SubNLG; + typedef partition::PartitionTable PartitionTable; + Graph fg; + fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise); + fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise); - fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); - fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); - fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); - fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); - fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); - fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); - fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); + fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); + fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); - fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise); + fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise); - // generate ordering - Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6; + // generate ordering + Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6; - // define cuts - boost::shared_ptr cuts(new Cuts()); - cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable; - //x0 x1 x2 l1 l2 l3 l4 l5 l6 - (*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2; + // define cuts + boost::shared_ptr cuts(new Cuts()); + cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2; - cuts->children.push_back(boost::shared_ptr(new Cuts())); - cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable; - //x0 x1 x2 l1 l2 l3 l4 l5 l6 - (*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1; + cuts->children.push_back(boost::shared_ptr(new Cuts())); + cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1; - cuts->children.push_back(boost::shared_ptr(new Cuts())); - cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable; - //x0 x1 x2 l1 l2 l3 l4 l5 l6 - (*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2; + cuts->children.push_back(boost::shared_ptr(new Cuts())); + cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable; + //x0 x1 x2 l1 l2 l3 l4 l5 l6 + (*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2; - // nested dissection - NestedDissection nd(fg, ordering, cuts); - LONGS_EQUAL(2, nd.root()->size()); - LONGS_EQUAL(3, nd.root()->frontal().size()); - LONGS_EQUAL(0, nd.root()->separator().size()); - LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps - LONGS_EQUAL(0, nd.root()->weeklinks().size()); + // nested dissection + NestedDissection nd(fg, ordering, cuts); + LONGS_EQUAL(2, nd.root()->size()); + LONGS_EQUAL(3, nd.root()->frontal().size()); + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + LONGS_EQUAL(0, nd.root()->weeklinks().size()); - // the 1st submap - LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0 - LONGS_EQUAL(4, nd.root()->children()[0]->size()); - LONGS_EQUAL(2, nd.root()->children()[0]->children().size()); + // the 1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0 + LONGS_EQUAL(4, nd.root()->children()[0]->size()); + LONGS_EQUAL(2, nd.root()->children()[0]->children().size()); - // the 1-1st submap - LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1 - LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size()); + // the 1-1st submap + LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1 + LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size()); - // the 1-2nd submap - LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4 - LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size()); + // the 1-2nd submap + LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4 + LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size()); - // the 2nd submap - LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2 - LONGS_EQUAL(3, nd.root()->children()[1]->size()); - LONGS_EQUAL(2, nd.root()->children()[1]->children().size()); + // the 2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2 + LONGS_EQUAL(3, nd.root()->children()[1]->size()); + LONGS_EQUAL(2, nd.root()->children()[1]->children().size()); - // the 2-1st submap - LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3 - LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size()); + // the 2-1st submap + LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3 + LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size()); - // the 2-2nd submap - LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6 - LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size()); + // the 2-2nd submap + LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6 + LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size()); } @@ -272,65 +272,65 @@ TEST ( NestedDissection, manual_cuts ) // / | / \ | \ // x0 x1 x2 x3 TEST( NestedDissection, Graph3D) { - using namespace gtsam::submapVisualSLAM; - typedef TSAM3D::SubNLG SubNLG; - typedef partition::PartitionTable PartitionTable; - vector cameras; - cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.)))); - cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.)))); - cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.)))); - cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.)))); + using namespace gtsam::submapVisualSLAM; + typedef TSAM3D::SubNLG SubNLG; + typedef partition::PartitionTable PartitionTable; + vector cameras; + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.)))); + cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.)))); - vector points; - for (int cube_index = 0; cube_index <= 3; cube_index++) { - Point3 center((cube_index-1) * 3, 0.5, 10.); - points.push_back(center + Point3(-0.5, -0.5, -0.5)); - points.push_back(center + Point3(-0.5, 0.5, -0.5)); - points.push_back(center + Point3( 0.5, 0.5, -0.5)); - points.push_back(center + Point3( 0.5, -0.5, -0.5)); - points.push_back(center + Point3(-0.5, -0.5, 0.5)); - points.push_back(center + Point3(-0.5, 0.5, 0.5)); - points.push_back(center + Point3( 0.5, 0.5, 0.5)); - points.push_back(center + Point3( 0.5, 0.5, 0.5)); - } + vector points; + for (int cube_index = 0; cube_index <= 3; cube_index++) { + Point3 center((cube_index-1) * 3, 0.5, 10.); + points.push_back(center + Point3(-0.5, -0.5, -0.5)); + points.push_back(center + Point3(-0.5, 0.5, -0.5)); + points.push_back(center + Point3( 0.5, 0.5, -0.5)); + points.push_back(center + Point3( 0.5, -0.5, -0.5)); + points.push_back(center + Point3(-0.5, -0.5, 0.5)); + points.push_back(center + Point3(-0.5, 0.5, 0.5)); + points.push_back(center + Point3( 0.5, 0.5, 0.5)); + points.push_back(center + Point3( 0.5, 0.5, 0.5)); + } - Graph graph; - SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.)); - SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.)); - for (int j=1; j<=8; j++) - graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); - for (int j=1; j<=16; j++) - graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); - for (int j=9; j<=24; j++) - graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); - for (int j=17; j<=24; j++) - graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + Graph graph; + SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.)); + SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.)); + for (int j=1; j<=8; j++) + graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=1; j<=16; j++) + graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=9; j<=24; j++) + graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); + for (int j=17; j<=24; j++) + graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); - // make an easy ordering - Ordering ordering; ordering += x0, x1, x2, x3; - for (int j=1; j<=24; j++) - ordering += Symbol('l', j); + // make an easy ordering + Ordering ordering; ordering += x0, x1, x2, x3; + for (int j=1; j<=24; j++) + ordering += Symbol('l', j); - // nested dissection - const int numNodeStopPartition = 10; - const int minNodesPerMap = 5; - NestedDissection nd(graph, ordering, numNodeStopPartition, minNodesPerMap); + // nested dissection + const int numNodeStopPartition = 10; + const int minNodesPerMap = 5; + NestedDissection nd(graph, ordering, numNodeStopPartition, minNodesPerMap); - LONGS_EQUAL(0, nd.root()->size()); - LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16 - LONGS_EQUAL(0, nd.root()->separator().size()); - LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps - LONGS_EQUAL(0, nd.root()->weeklinks().size()); + LONGS_EQUAL(0, nd.root()->size()); + LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16 + LONGS_EQUAL(0, nd.root()->separator().size()); + LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps + LONGS_EQUAL(0, nd.root()->weeklinks().size()); - // the 1st submap - LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8 - LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16 - LONGS_EQUAL(0, nd.root()->children()[0]->children().size()); + // the 1st submap + LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8 + LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16 + LONGS_EQUAL(0, nd.root()->children()[0]->children().size()); - // the 2nd submap - LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8 - LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8 - LONGS_EQUAL(0, nd.root()->children()[1]->children().size()); + // the 2nd submap + LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8 + LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8 + LONGS_EQUAL(0, nd.root()->children()[1]->children().size()); } diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 6c89f4c03..c147552b3 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -295,87 +295,87 @@ namespace gtsam { /* ************************************************************************* */ SharedGaussian get_model_inlier() const { - return model_inlier_; + return model_inlier_; } /* ************************************************************************* */ SharedGaussian get_model_outlier() const { - return model_outlier_; + return model_outlier_; } /* ************************************************************************* */ Matrix get_model_inlier_cov() const { - return (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); + return (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); } /* ************************************************************************* */ Matrix get_model_outlier_cov() const { - return (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); + return (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); } /* ************************************************************************* */ void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){ - /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories - * (note these are given in the E step, where indicator probabilities are calculated). - * - * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the - * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). - * - * TODO: improve efficiency (info form) - */ + /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories + * (note these are given in the E step, where indicator probabilities are calculated). + * + * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the + * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). + * + * TODO: improve efficiency (info form) + */ - // get joint covariance of the involved states - std::vector Keys; - Keys.push_back(key1_); - Keys.push_back(key2_); - Marginals marginals( graph, values, Marginals::QR ); - JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys); - Matrix cov1 = joint_marginal12(key1_, key1_); - Matrix cov2 = joint_marginal12(key2_, key2_); - Matrix cov12 = joint_marginal12(key1_, key2_); + // get joint covariance of the involved states + std::vector Keys; + Keys.push_back(key1_); + Keys.push_back(key2_); + Marginals marginals( graph, values, Marginals::QR ); + JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys); + Matrix cov1 = joint_marginal12(key1_, key1_); + Matrix cov2 = joint_marginal12(key2_, key2_); + Matrix cov12 = joint_marginal12(key1_, key2_); - updateNoiseModels_givenCovs(values, cov1, cov2, cov12); + updateNoiseModels_givenCovs(values, cov1, cov2, cov12); } /* ************************************************************************* */ void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){ - /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories - * (note these are given in the E step, where indicator probabilities are calculated). - * - * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the - * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). - * - * TODO: improve efficiency (info form) - */ + /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories + * (note these are given in the E step, where indicator probabilities are calculated). + * + * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the + * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). + * + * TODO: improve efficiency (info form) + */ - const T& p1 = values.at(key1_); - const T& p2 = values.at(key2_); + const T& p1 = values.at(key1_); + const T& p2 = values.at(key2_); - Matrix H1, H2; - T hx = p1.between(p2, H1, H2); // h(x) + Matrix H1, H2; + T hx = p1.between(p2, H1, H2); // h(x) - Matrix H; - H.resize(H1.rows(), H1.rows()+H2.rows()); - H << H1, H2; // H = [H1 H2] + Matrix H; + H.resize(H1.rows(), H1.rows()+H2.rows()); + H << H1, H2; // H = [H1 H2] - Matrix joint_cov; - joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols()); - joint_cov << cov1, cov12, - cov12.transpose(), cov2; + Matrix joint_cov; + joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols()); + joint_cov << cov1, cov12, + cov12.transpose(), cov2; - Matrix cov_state = H*joint_cov*H.transpose(); + Matrix cov_state = H*joint_cov*H.transpose(); - // model_inlier_->print("before:"); + // model_inlier_->print("before:"); - // update inlier and outlier noise models - Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); - model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state); + // update inlier and outlier noise models + Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); + model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state); - Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); - model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state); + Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); + model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state); - // model_inlier_->print("after:"); - // std::cout<<"covRinlier + cov_state: "<print("after:"); + // std::cout<<"covRinlier + cov_state: "< (&expected); + const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol); } diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 9fb7942e7..b3df86886 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -256,43 +256,43 @@ TEST( BetweenFactorEM, CaseStudy) ///* ************************************************************************** */ TEST (BetweenFactorEM, updateNoiseModel ) { - gtsam::Key key1(1); - gtsam::Key key2(2); + gtsam::Key key1(1); + gtsam::Key key2(2); - gtsam::Pose2 p1(10.0, 15.0, 0.1); - gtsam::Pose2 p2(15.0, 15.0, 0.3); - gtsam::Pose2 noise(0.5, 0.4, 0.01); - gtsam::Pose2 rel_pose_ideal = p1.between(p2); - gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); + gtsam::Pose2 p1(10.0, 15.0, 0.1); + gtsam::Pose2 p2(15.0, 15.0, 0.3); + gtsam::Pose2 noise(0.5, 0.4, 0.01); + gtsam::Pose2 rel_pose_ideal = p1.between(p2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); - SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 1.5, 2.5, 4.05))); - SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 50.0, 50.0, 10.0))); + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 1.5, 2.5, 4.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 50.0, 50.0, 10.0))); - gtsam::Values values; - values.insert(key1, p1); - values.insert(key2, p2); + gtsam::Values values; + values.insert(key1, p1); + values.insert(key2, p2); - double prior_outlier = 0.0; - double prior_inlier = 1.0; + double prior_outlier = 0.0; + double prior_inlier = 1.0; - BetweenFactorEM f(key1, key2, rel_pose_msr, model_inlier, model_outlier, - prior_inlier, prior_outlier); + BetweenFactorEM f(key1, key2, rel_pose_msr, model_inlier, model_outlier, + prior_inlier, prior_outlier); - SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2)); + SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2)); - NonlinearFactorGraph graph; - graph.push_back(gtsam::PriorFactor(key1, p1, model)); - graph.push_back(gtsam::PriorFactor(key2, p2, model)); + NonlinearFactorGraph graph; + graph.push_back(gtsam::PriorFactor(key1, p1, model)); + graph.push_back(gtsam::PriorFactor(key2, p2, model)); - f.updateNoiseModels(values, graph); + f.updateNoiseModels(values, graph); - SharedGaussian model_inlier_new = f.get_model_inlier(); - SharedGaussian model_outlier_new = f.get_model_outlier(); + SharedGaussian model_inlier_new = f.get_model_inlier(); + SharedGaussian model_outlier_new = f.get_model_outlier(); - model_inlier->print("model_inlier:"); - model_outlier->print("model_outlier:"); - model_inlier_new->print("model_inlier_new:"); - model_outlier_new->print("model_outlier_new:"); + model_inlier->print("model_inlier:"); + model_outlier->print("model_outlier:"); + model_inlier_new->print("model_inlier_new:"); + model_outlier_new->print("model_outlier_new:"); }