gtsam/gtsam_unstable/partition/GenericGraph.cpp

/*
 * GenericGraph2D.cpp
 *
 *   Created on: Nov 23, 2010
 *       Author: nikai
 *  Description: generic graph types used in partitioning
 */
#include <iostream>
#include <cassert>
#include <algorithm>

#include <gtsam/base/DSFVector.h>

#include "GenericGraph.h"

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<vector<size_t> > findIslands(const GenericGraph2D& graph, const vector<size_t>& keys, WorkSpace& workspace,
      const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
  {
    typedef pair<int, int> IntPair;
    typedef list<sharedGenericFactor2D> FactorList;
    typedef map<IntPair, FactorList::iterator> Connections;

    // 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<FactorList::iterator> 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;
        }

        size_t label1 = dsf.find(key1.index);
        size_t label2 = dsf.find(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.merge(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.merge(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.merge(label1, label2);
            connections.erase(itCached);
            succeed = true;
            break;
          }
        }
      }

      // erase unused factors
      for(const FactorList::iterator& it: toErase)
        factors.erase(it);

      if (!succeed) break;
    }

    list<vector<size_t> > islands;
    map<size_t, vector<size_t> > arrays = dsf.arrays();
    for(const auto& kv : arrays)
      islands.push_back(kv.second);
    return islands;
  }


  /* ************************************************************************* */
  void print(const GenericGraph2D& graph, const std::string name) {
    cout << name << endl;
    for(const sharedGenericFactor2D& factor_: graph)
      cout << factor_->key1.index << " " << factor_->key2.index << endl;
  }

  /* ************************************************************************* */
  void print(const GenericGraph3D& graph, const std::string name) {
    cout << name << endl;
    for(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<size_t>& keys, const WorkSpace& workspace) {
    DSFVector dsf(workspace.dsf, keys);
    typedef list<sharedGenericFactor3D> FactorList;

    FactorList factors(graph.begin(), graph.end());
    size_t nrFactors = factors.size();
    FactorList::iterator itEnd;
    while (nrFactors) {

      bool succeed = false;
      itEnd = factors.end();
      list<FactorList::iterator> 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;
        }

        if (graph.size() == 178765) cout << "kai22" << endl;
        size_t label1 = dsf.find(key1.index);
        size_t label2 = dsf.find(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.merge(label1, label2);
          succeed = true;
          break;
        }

        if (graph.size() == 178765) cout << "kai24" << endl;


      }

      // erase unused factors
      for(const FactorList::iterator& it: toErase)
      factors.erase(it);

      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<size_t>& singularCameras, const set<size_t>& 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<bool>& isCamera, const vector<bool>& isLandmark,
      set<size_t>& singularCameras, set<size_t>& singularLandmarks,  vector<int>& nrConstraints,
      bool& foundSingularCamera, bool& foundSingularLandmark,
      const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {

    // compute the constraint number per camera
    std::fill(nrConstraints.begin(),  nrConstraints.end(),    0);
    for(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;
        }
      }
    }

    // find singular cameras and landmarks
    foundSingularCamera = false;
    foundSingularLandmark = false;
    for (size_t i=0; i<nrConstraints.size(); i++) {
      if (isCamera[i] && nrConstraints[i] < minNrConstraintsPerCamera &&
          singularCameras.find(i) == singularCameras.end()) {
        singularCameras.insert(i);
        foundSingularCamera = true;
      }
      if (isLandmark[i] && nrConstraints[i] < minNrConstraintsPerLandmark &&
          singularLandmarks.find(i) == singularLandmarks.end()) {
        singularLandmarks.insert(i);
        foundSingularLandmark = true;
      }
    }
  }

  /* ************************************************************************* */
  list<vector<size_t> > findIslands(const GenericGraph3D& graph, const vector<size_t>& keys, WorkSpace& workspace,
      const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {

    // create disjoint set forest
    workspace.prepareDictionary(keys);
    DSFVector dsf = createDSF(graph, keys, workspace);

    const bool verbose = false;
    bool foundSingularCamera = true;
    bool foundSingularLandmark = true;

    list<vector<size_t> > islands;
    set<size_t> singularCameras, singularLandmarks;
    vector<bool> isCamera(workspace.dictionary.size(), false);
    vector<bool> isLandmark(workspace.dictionary.size(), false);

    // check the constraint number of every variable
    // find the camera and landmark keys
    for(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;
            }
    }

    vector<int> 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:";
      for(const size_t i: singularCameras) {
        islands.push_back(vector<size_t>(1, i)); // <---------------------------
        if (verbose) cout << i << " ";
      }
      if (verbose) cout << endl;
    }
    if (!singularLandmarks.empty()) {
      if (verbose) cout << "singular landmarks:";
      for(const size_t i: singularLandmarks) {
        islands.push_back(vector<size_t>(1, i)); // <---------------------------
        if (verbose) cout << i << " ";
      }
      if (verbose) cout << endl;
    }


    // regenerating islands
    map<size_t, vector<size_t> > labelIslands = dsf.arrays();
    size_t label; vector<size_t> island;
    for(const auto& li: labelIslands) {
      tie(label, island) = li;
      vector<size_t> filteredIsland; // remove singular cameras from array
      filteredIsland.reserve(island.size());
      for(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;
    for(const vector<size_t>& 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;
  }

  /* ************************************************************************* */
  // return the number of intersection between two **sorted** landmark vectors
  inline int getNrCommonLandmarks(const vector<size_t>& landmarks1, const vector<size_t>& 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<size_t>& cameraKeys,  const std::vector<size_t>& landmarkKeys,
      const std::vector<int>& dictionary,  GenericGraph3D& reducedGraph) {

    typedef size_t LandmarkKey;
    // get a mapping from each landmark to its connected cameras
    vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size());
    // for odometry xi-xj where i<j, we always store cameraToCamera[i] = j, otherwise equal to -1 if no odometry
    vector<int> cameraToCamera(dictionary.size(), -1);
    size_t key_i, key_j;
    for(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
    for(vector<LandmarkKey> &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<cameraKeys.size()-1; ++i1) {
      for (size_t i2=i1+1; i2<cameraKeys.size(); ++i2) {
        camera1 = cameraKeys[i1];
        camera2 = cameraKeys[i2];
        int nrCommonLandmarks = getNrCommonLandmarks(cameraToLandmarks[camera1], cameraToLandmarks[camera2]);
        hasOdometry =  cameraToCamera[camera1] == camera2;
        if (nrCommonLandmarks > 0 || hasOdometry) {
          nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0);
          reducedGraph.push_back(std::make_shared<GenericFactor3D>(camera1, camera2,
              factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints));
        }
      }
    }
  }

  /* ************************************************************************* */
  void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
      WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
    workspace.prepareDictionary(frontals);
    vector<size_t> nrConstraints(workspace.dictionary.size(), 0);

    // summarize the constraint number
    const vector<int>& dictionary = workspace.dictionary;
    vector<bool> isValidCamera(workspace.dictionary.size(), false);
    vector<bool> isValidLandmark(workspace.dictionary.size(), false);
    for(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;

      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;
      }
    }

    // find the minimum constraint for cameras and landmarks
    size_t minFoundConstraintsPerCamera = 10000;
    size_t minFoundConstraintsPerLandmark = 10000;

    for (size_t i=0; i<isValidCamera.size(); i++) {
      if (isValidCamera[i]) {
        minFoundConstraintsPerCamera   = std::min(nrConstraints[i], minFoundConstraintsPerCamera);
        if (nrConstraints[i] < minNrConstraintsPerCamera)
              cout << "!!!!!!!!!!!!!!!!!!! camera with " << nrConstraints[i] << " constraint: " << i << endl;
      }

    }
    for (size_t j=0; j<isValidLandmark.size(); j++) {
      if (isValidLandmark[j]) {
        minFoundConstraintsPerLandmark = std::min(nrConstraints[j], minFoundConstraintsPerLandmark);
        if (nrConstraints[j] < minNrConstraintsPerLandmark)
          cout << "!!!!!!!!!!!!!!!!!!! landmark with " << nrConstraints[j] << " constraint: " << j << endl;
      }
    }

    // debug info
    for(const size_t key: frontals) {
      if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera)
        cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl;
    }

     if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera)
      throw runtime_error("checkSingularity:minConstraintsPerCamera < " + std::to_string(minFoundConstraintsPerCamera));
    if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark)
      throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + std::to_string(minFoundConstraintsPerLandmark));
  }

}} // namespace