nonlinear/utilities.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

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>

#include <exception>

namespace gtsam {

namespace utilities {

// Create a KeyList from indices
FastList<Key> createKeyList(const Vector& I) {
  FastList<Key> set;
  for (int i = 0; i < I.size(); i++)
    set.push_back(I[i]);
  return set;
}

// Create a KeyList from indices using symbol
FastList<Key> createKeyList(std::string s, const Vector& I) {
  FastList<Key> set;
  char c = s[0];
  for (int i = 0; i < I.size(); i++)
    set.push_back(Symbol(c, I[i]));
  return set;
}

// Create a KeyVector from indices
KeyVector createKeyVector(const Vector& I) {
  KeyVector set;
  for (int i = 0; i < I.size(); i++)
    set.push_back(I[i]);
  return set;
}

// Create a KeyVector from indices using symbol
KeyVector createKeyVector(std::string s, const Vector& I) {
  KeyVector set;
  char c = s[0];
  for (int i = 0; i < I.size(); i++)
    set.push_back(Symbol(c, I[i]));
  return set;
}

// Create a KeySet from indices
KeySet createKeySet(const Vector& I) {
  KeySet set;
  for (int i = 0; i < I.size(); i++)
    set.insert(I[i]);
  return set;
}

// Create a KeySet from indices using symbol
KeySet createKeySet(std::string s, const Vector& I) {
  KeySet set;
  char c = s[0];
  for (int i = 0; i < I.size(); i++)
    set.insert(symbol(c, I[i]));
  return set;
}

Matrix extractPoint2(const Values& values) {
  const auto points = values.extract<gtsam::Point2>();
  // Point2 is aliased as a gtsam::Vector in the wrapper
  const auto points2 = values.extract<gtsam::Vector>();

  Matrix result(points.size() + points2.size(), 2);

  size_t j = 0;
  for (const auto& key_value : points) {
    result.row(j++) = key_value.second;
  }
  for (const auto& key_value : points2) {
    if (key_value.second.rows() == 2) {
      result.row(j++) = key_value.second;
    }
  }
  return result;
}

Matrix extractPoint3(const Values& values) {
  const auto points = values.extract<gtsam::Point3>();
  // Point3 is aliased as a gtsam::Vector in the wrapper
  const auto points2 = values.extract<gtsam::Vector>();

  Matrix result(points.size() + points2.size(), 3);

  size_t j = 0;
  for (const auto& key_value : points) {
    result.row(j++) = key_value.second;
  }
  for (const auto& key_value : points2) {
    if (key_value.second.rows() == 3) {
      result.row(j++) = key_value.second;
    }
  }
  return result;
}

Values allPose2s(const Values& values) {
  Values result;
  for(const auto& key_value: values.extract<Pose2>())
    result.insert(key_value.first, key_value.second);
  return result;
}

Matrix extractPose2(const Values& values) {
  const auto poses = values.extract<Pose2>();
  Matrix result(poses.size(), 3);
  size_t j = 0;
  for(const auto& key_value: poses)
    result.row(j++) << key_value.second.x(), key_value.second.y(), key_value.second.theta();
  return result;
}

Values allPose3s(const Values& values) {
  Values result;
  for(const auto& key_value: values.extract<Pose3>())
    result.insert(key_value.first, key_value.second);
  return result;
}

Matrix extractPose3(const Values& values) {
  const auto poses = values.extract<Pose3>();
  Matrix result(poses.size(), 12);
  size_t j = 0;
  for(const auto& key_value: poses) {
    result.row(j).segment(0, 3) << key_value.second.rotation().matrix().row(0);
    result.row(j).segment(3, 3) << key_value.second.rotation().matrix().row(1);
    result.row(j).segment(6, 3) << key_value.second.rotation().matrix().row(2);
    result.row(j).tail(3) = key_value.second.translation();
    j++;
  }
  return result;
}

Matrix extractVectors(const Values& values, char c) {
  const auto vectors = values.extract<Vector>(Symbol::ChrTest(c));
  if (vectors.size() == 0) {
    return Matrix();
  }
  auto dim = vectors.begin()->second.size();
  Matrix result(vectors.size(), dim);
  Eigen::Index rowi = 0;
  for (const auto& kv : vectors) {
    if (kv.second.size() != dim) {
      throw std::runtime_error(
          "Tried to extract different-sized vectors into a single matrix");
    }
    result.row(rowi) = kv.second;
    ++rowi;
  }
  return result;
}

void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) {
  noiseModel::Isotropic::shared_ptr model =
      noiseModel::Isotropic::Sigma(2, sigma);
  Sampler sampler(model, seed);
  for (const auto& key_value : values.extract<Point2>()) {
    values.update<Point2>(key_value.first,
                          key_value.second + Point2(sampler.sample()));
  }
  for (const auto& key_value : values.extract<gtsam::Vector>()) {
    if (key_value.second.rows() == 2) {
      values.update<gtsam::Vector>(key_value.first,
                                   key_value.second + Point2(sampler.sample()));
    }
  }
}

void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed =
    42u) {
  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(
      Vector3(sigmaT, sigmaT, sigmaR));
  Sampler sampler(model, seed);
  for(const auto& key_value: values.extract<Pose2>()) {
    values.update<Pose2>(key_value.first, key_value.second.retract(sampler.sample()));
  }
}

void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
  noiseModel::Isotropic::shared_ptr model =
      noiseModel::Isotropic::Sigma(3, sigma);
  Sampler sampler(model, seed);
  for (const auto& key_value : values.extract<Point3>()) {
    values.update<Point3>(key_value.first,
                          key_value.second + Point3(sampler.sample()));
  }
  for (const auto& key_value : values.extract<gtsam::Vector>()) {
    if (key_value.second.rows() == 3) {
      values.update<gtsam::Vector>(key_value.first,
                                   key_value.second + Point3(sampler.sample()));
    }
  }
}

void insertBackprojections(Values& values, const PinholeCamera<Cal3_S2>& camera,
    const Vector& J, const Matrix& Z, double depth) {
  if (Z.rows() != 2)
    throw std::invalid_argument("insertBackProjections: Z must be 2*J");
  if (Z.cols() != J.size())
    throw std::invalid_argument(
        "insertBackProjections: J and Z must have same number of entries");
  for (int k = 0; k < Z.cols(); k++) {
    Point2 p(Z(0, k), Z(1, k));
    Point3 P = camera.backproject(p, depth);
    values.insert(J(k), P);
  }
}

void insertProjectionFactors(NonlinearFactorGraph& graph, Key i,
    const Vector& J, const Matrix& Z, const SharedNoiseModel& model,
    const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) {
  if (Z.rows() != 2)
    throw std::invalid_argument("addMeasurements: Z must be 2*K");
  if (Z.cols() != J.size())
    throw std::invalid_argument(
        "addMeasurements: J and Z must have same number of entries");
  for (int k = 0; k < Z.cols(); k++) {
    graph.push_back(
        std::make_shared<GenericProjectionFactor<Pose3, Point3> >(
            Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor));
  }
}

Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
    const Values& values) {
  // first count
  size_t K = 0, k = 0;
  for(const NonlinearFactor::shared_ptr& f: graph)
    if (std::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
        f))
      ++K;
  // now fill
  Matrix errors(2, K);
  for(const NonlinearFactor::shared_ptr& f: graph) {
    std::shared_ptr<const GenericProjectionFactor<Pose3, Point3> > p =
        std::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
            f);
    if (p)
      errors.col(k++) = p->unwhitenedError(values);
  }
  return errors;
}

Values localToWorld(const Values& local, const Pose2& base,
    const KeyVector user_keys = KeyVector()) {

  Values world;

  // if no keys given, get all keys from local values
  KeyVector keys(user_keys);
  if (keys.size()==0)
    keys = local.keys();

  // Loop over all keys
  for(Key key: keys) {
    try {
      // if value is a Pose2, compose it with base pose
      Pose2 pose = local.at<Pose2>(key);
      world.insert(key, base.compose(pose));
    } catch ([[maybe_unused]] const std::exception& e1) {
      try {
        // if value is a Point2, transform it from base pose
        Point2 point = local.at<Point2>(key);
        world.insert(key, base.transformFrom(point));
      } catch ([[maybe_unused]]  const std::exception& e2) {
        // if not Pose2 or Point2, do nothing
        #ifndef NDEBUG
          std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl;
        #endif
      }
    }
  }
  return world;
}

} // namespace utilities

}