Switching.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */
 
/*
 *  @file Switching.h
 *  @date Mar 11, 2022
 *  @author Varun Agrawal
 *  @author Fan Jiang
 */
 
#include <gtsam/base/Matrix.h>
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteDistribution.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
 
#include <vector>
 
#pragma once
 
namespace gtsam {
 
using symbol_shorthand::C;
using symbol_shorthand::M;
using symbol_shorthand::X;
 
inline HybridGaussianFactorGraph::shared_ptr makeSwitchingChain(
    size_t K, std::function<Key(int)> x = X, std::function<Key(int)> m = M) {
  HybridGaussianFactorGraph hfg;
 
  hfg.add(JacobianFactor(x(1), I_3x3, Z_3x1));
 
  // x(1) to x(n+1)
  for (size_t k = 1; k < K; k++) {
    DiscreteKeys dKeys{{m(k), 2}};
    std::vector<GaussianFactor::shared_ptr> components;
    components.emplace_back(
        new JacobianFactor(x(k), I_3x3, x(k + 1), I_3x3, Z_3x1));
    components.emplace_back(
        new JacobianFactor(x(k), I_3x3, x(k + 1), I_3x3, Vector3::Ones()));
    hfg.add(HybridGaussianFactor({m(k), 2}, components));
 
    if (k > 1) {
      hfg.add(DecisionTreeFactor({{m(k - 1), 2}, {m(k), 2}}, "0 1 1 3"));
    }
  }
 
  return std::make_shared<HybridGaussianFactorGraph>(std::move(hfg));
}
 
inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
    std::vector<Key> &input) {
  KeyVector new_order;
 
  std::vector<int> levels(input.size());
  std::function<void(std::vector<Key>::iterator, std::vector<Key>::iterator,
                     int)>
      bsg = [&bsg, &new_order, &levels, &input](
                std::vector<Key>::iterator begin,
                std::vector<Key>::iterator end, int lvl) {
        if (std::distance(begin, end) > 1) {
          std::vector<Key>::iterator pivot =
              begin + std::distance(begin, end) / 2;
 
          new_order.push_back(*pivot);
          levels[std::distance(input.begin(), pivot)] = lvl;
          bsg(begin, pivot, lvl + 1);
          bsg(pivot + 1, end, lvl + 1);
        } else if (std::distance(begin, end) == 1) {
          new_order.push_back(*begin);
          levels[std::distance(input.begin(), begin)] = lvl;
        }
      };
 
  bsg(input.begin(), input.end(), 0);
  std::reverse(new_order.begin(), new_order.end());
 
  return {new_order, levels};
}
 
/* ****************************************************************************/
using MotionModel = BetweenFactor<double>;
 
// Test fixture with switching network.
struct Switching {
 public:
  size_t K;
  DiscreteKeys modes;
  HybridNonlinearFactorGraph unaryFactors, binaryFactors, modeChain;
  HybridGaussianFactorGraph linearUnaryFactors, linearBinaryFactors;
  Values linearizationPoint;
 
  Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1,
            std::vector<double> measurements = {},
            std::string transitionProbabilityTable = "1/2 3/2")
      : K(K) {
    using noiseModel::Isotropic;
 
    // Create DiscreteKeys for K-1 binary modes.
    for (size_t k = 0; k < K - 1; k++) {
      modes.emplace_back(M(k), 2);
    }
 
    // If measurements are not provided, we just have the robot moving forward.
    if (measurements.size() == 0) {
      for (size_t k = 0; k < K; k++) {
        measurements.push_back(k);
      }
    }
 
    // Create hybrid factor graph.
 
    // Add a prior ϕ(X(0)) on X(0).
    unaryFactors.emplace_shared<PriorFactor<double>>(
        X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma));
 
    // Add "motion models" ϕ(X(k),X(k+1),M(k)).
    for (size_t k = 0; k < K - 1; k++) {
      auto motion_models = motionModels(k, between_sigma);
      binaryFactors.emplace_shared<HybridNonlinearFactor>(modes[k],
                                                          motion_models);
    }
 
    // Add measurement factors ϕ(X(k);z_k).
    auto measurement_noise = Isotropic::Sigma(1, prior_sigma);
    for (size_t k = 1; k < K; k++) {
      unaryFactors.emplace_shared<PriorFactor<double>>(X(k), measurements.at(k),
                                                       measurement_noise);
    }
 
    // Add "mode chain" ϕ(M(0)) ϕ(M(0),M(1)) ... ϕ(M(K-3),M(K-2))
    modeChain = createModeChain(transitionProbabilityTable);
 
    // Create the linearization point.
    for (size_t k = 0; k < K; k++) {
      linearizationPoint.insert<double>(X(k), static_cast<double>(k + 1));
    }
 
    linearUnaryFactors = *unaryFactors.linearize(linearizationPoint);
    linearBinaryFactors = *binaryFactors.linearize(linearizationPoint);
  }
 
  // Create motion models for a given time step
  static std::vector<NoiseModelFactor::shared_ptr> motionModels(
      size_t k, double sigma = 1.0) {
    auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
    auto still =
             std::make_shared<MotionModel>(X(k), X(k + 1), 0.0, noise_model),
         moving =
             std::make_shared<MotionModel>(X(k), X(k + 1), 1.0, noise_model);
    return {still, moving};
  }
 
  HybridNonlinearFactorGraph createModeChain(
      std::string transitionProbabilityTable = "1/2 3/2") {
    HybridNonlinearFactorGraph chain;
    chain.emplace_shared<DiscreteDistribution>(modes[0], "1/1");
    for (size_t k = 0; k < K - 2; k++) {
      auto parents = {modes[k]};
      chain.emplace_shared<DiscreteConditional>(modes[k + 1], parents,
                                                transitionProbabilityTable);
    }
    return chain;
  }
 
  HybridGaussianFactorGraph linearizedFactorGraph() const {
    HybridGaussianFactorGraph graph;
    graph.push_back(linearUnaryFactors);
    graph.push_back(linearBinaryFactors);
    graph.push_back(modeChain);
    return graph;
  }
 
  HybridNonlinearFactorGraph nonlinearFactorGraph() const {
    HybridNonlinearFactorGraph graph;
    graph.push_back(unaryFactors);
    graph.push_back(binaryFactors);
    graph.push_back(modeChain);
    return graph;
  }
};
 
}  // namespace gtsam