QuadraticRangeFactor.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/base/Matrix.h>
#include <gtsam/constrained/QcqpProblem.h>
#include <gtsam/constrained/QpCost.h>
#include <gtsam/constrained/QuadraticConstraint.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
 
#include <cmath>
#include <stdexcept>
#include <string>
#include <type_traits>
 
namespace gtsam {
 
template <int d>
class QuadraticRangeFactor
    : public NoiseModelFactorN<
          Eigen::Matrix<double, d, 1>, Eigen::Matrix<double, d, 1>,
          typename std::conditional<d == 2, Rot2, Unit3>::type> {
  static_assert(d == 2 || d == 3, "QuadraticRangeFactor supports d = 2 or 3.");
 
  using Point = Eigen::Matrix<double, d, 1>;
  using Direction = typename std::conditional<d == 2, Rot2, Unit3>::type;
  using Base = NoiseModelFactorN<Point, Point, Direction>;
 
  double range_;   
  double weight_;  
 
 public:
  static constexpr int kDirectionRows = (d == 2) ? 2 : 1;
 
  using Base::evaluateError;

  QuadraticRangeFactor(Key translationKey, Key targetKey, Key unitVectorKey,
                       double range, double weight)
      : Base(noiseModel::Unit::Create(d), translationKey, targetKey,
             unitVectorKey),
        range_(range),
        weight_(weight) {
    if (weight <= 0.0) {
      throw std::invalid_argument(
          "QuadraticRangeFactor: weight must be positive.");
    }
    if (range < 0.0) {
      throw std::invalid_argument(
          "QuadraticRangeFactor: range must be non-negative.");
    }
  }
 
  double range() const { return range_; }
  double weight() const { return weight_; }
 
  void print(
      const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    std::cout << s << "QuadraticRangeFactor(" << keyFormatter(this->key1())
              << "," << keyFormatter(this->key2()) << ","
              << keyFormatter(this->key3()) << ") range=" << range_
              << " weight=" << weight_ << "\n";
  }
 
  bool equals(const NonlinearFactor& other, double tol = 1e-9) const override {
    const auto* e = dynamic_cast<const QuadraticRangeFactor*>(&other);
    return e != nullptr && Base::equals(other, tol) &&
           std::abs(range_ - e->range_) < tol &&
           std::abs(weight_ - e->weight_) < tol;
  }

  Vector evaluateError(const Point& translation, const Point& target,
                       const Direction& direction, OptionalMatrixType H1,
                       OptionalMatrixType H2,
                       OptionalMatrixType H3) const override {
    const double sw = std::sqrt(weight_);
    const Point u = [&direction] {
      if constexpr (d == 2)
        return Point(direction.c(), direction.s());
      else
        return direction.unitVector();
    }();
    if (H1) *H1 = -sw * Matrix::Identity(d, d);
    if (H2) *H2 = sw * Matrix::Identity(d, d);
    if (H3) {
      Matrix J = Matrix::Zero(d, Direction::dimension);
      if constexpr (d == 2) {
        J(0, 0) = -u(1);
        J(1, 0) = u(0);
      } else {
        J = direction.basis();
      }
      *H3 = -sw * range_ * J;
    }
    return sw * (target - translation - range_ * u);
  }
 
  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new QuadraticRangeFactor(*this)));
  }

  void qcqpFactors(NonlinearFactorGraph* costs,
                   NonlinearEqualityConstraints* constraints,
                   size_t columnDimension = 1) const override {
    if (columnDimension == 0) {
      throw std::invalid_argument(
          "QuadraticRangeFactor::qcqpFactors requires a positive "
          "columnDimension.");
    }
    if (columnDimension == 1) {
      qcqpFactorsForVector(costs, constraints);
      return;
    }
    if (columnDimension < static_cast<size_t>(d)) {
      throw std::invalid_argument(
          "QuadraticRangeFactor::qcqpFactors requires columnDimension "
          ">= d.");
    }
    if (!costs) {
      throw std::invalid_argument(
          "QuadraticRangeFactor::qcqpFactors: costs is null.");
    }
 
    InsertQcqpConstraints<Direction, d>(this->key3(), constraints);
 
    // The direction is a Rot2 in 2D, which takes 2 rows, and a Unit3 in 3D,
    // which takes 1. Only the first row is used; any others stay zero.
    const double sw = std::sqrt(weight_);
    Matrix B = Matrix::Zero(1, 2 + kDirectionRows);
    B(0, 0) = -sw;           // pose translation
    B(0, 1) = sw;            // target
    B(0, 2) = -sw * range_;  // leading row of the auxiliary direction
 
    const Matrix Q = B.transpose() * B;
    const SymmetricBlockMatrix blockQ(
        std::vector<DenseIndex>{1, 1, kDirectionRows}, Q);
    costs->push_back(std::make_shared<QpCost>(
        KeyVector{this->key1(), this->key2(), this->key3()}, blockQ,
        columnDimension));
  }
 
 private:
  void qcqpFactorsForVector(NonlinearFactorGraph* costs,
                            NonlinearEqualityConstraints* constraints) const {
    if (!costs) {
      throw std::invalid_argument(
          "QuadraticRangeFactor::qcqpFactors: costs is null.");
    }
 
    constexpr int kPointDim = traits<Point>::QcqpVectorDim;
    constexpr int kDirectionDim = traits<Direction>::QcqpVectorDim;
    const double sw = std::sqrt(weight_);
 
    // Both Rot2 and Unit3 store the relevant direction in the d entries after
    // the homogeneous coordinate (Rot2's first matrix column in 2D).
    Matrix directionSelector = Matrix::Zero(d, kDirectionDim);
    directionSelector.block(0, 1, d, d).setIdentity();
 
    Matrix B = Matrix::Zero(d, 2 * kPointDim + kDirectionDim);
    B.block(0, 1, d, d) = -sw * Matrix::Identity(d, d);
    B.block(0, kPointDim + 1, d, d) = sw * Matrix::Identity(d, d);
    B.block(0, 2 * kPointDim, d, kDirectionDim) =
        -sw * range_ * directionSelector;
 
    InsertQcqpConstraints<Point, 1>(this->key1(), constraints);
    InsertQcqpConstraints<Point, 1>(this->key2(), constraints);
    InsertQcqpConstraints<Direction, 1>(this->key3(), constraints);
 
    const Matrix Q = B.transpose() * B;
    const SymmetricBlockMatrix blockQ(
        std::vector<DenseIndex>{kPointDim, kPointDim, kDirectionDim}, Q);
    costs->push_back(std::make_shared<QpCost>(
        KeyVector{this->key1(), this->key2(), this->key3()}, blockQ));
  }
};
 
using QuadraticRangeFactor2 = QuadraticRangeFactor<2>;
using QuadraticRangeFactor3 = QuadraticRangeFactor<3>;
 
}  // namespace gtsam