RelativeTranslationFactor.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/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.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 RelativeTranslationFactor
    : public NoiseModelFactorN<
          typename std::conditional<d == 2, Rot2, Rot3>::type,
          Eigen::Matrix<double, d, 1>, Eigen::Matrix<double, d, 1>> {
  static_assert(d == 2 || d == 3,
                "RelativeTranslationFactor supports d = 2 or 3.");
 
  using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
  using Point = Eigen::Matrix<double, d, 1>;
  using Base = NoiseModelFactorN<Rot, Point, Point>;
 
  Point measured_;  
  double weight_;   
 
 public:
  using Base::evaluateError;

  RelativeTranslationFactor(Key rotationKey, Key translationKey1,
                            Key translationKey2, const Point& measured,
                            double weight)
      : Base(noiseModel::Unit::Create(d), rotationKey, translationKey1,
             translationKey2),
        measured_(measured),
        weight_(weight) {
    if (weight <= 0.0) {
      throw std::invalid_argument(
          "RelativeTranslationFactor: weight must be positive.");
    }
  }
 
  const Point& measured() const { return measured_; }
  double weight() const { return weight_; }
 
  void print(
      const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    std::cout << s << "RelativeTranslationFactor(" << keyFormatter(this->key1())
              << "," << keyFormatter(this->key2()) << ","
              << keyFormatter(this->key3()) << ") weight=" << weight_
              << " measured=[" << measured_.transpose() << "]\n";
  }
 
  bool equals(const NonlinearFactor& other, double tol = 1e-9) const override {
    const auto* e = dynamic_cast<const RelativeTranslationFactor*>(&other);
    return e != nullptr && Base::equals(other, tol) &&
           traits<Point>::Equals(measured_, e->measured_, tol) &&
           std::abs(weight_ - e->weight_) < tol;
  }

  Vector evaluateError(const Rot& Ri, const Point& ti, const Point& tj,
                       OptionalMatrixType H1, OptionalMatrixType H2,
                       OptionalMatrixType H3) const override {
    const double sw = std::sqrt(weight_);
 
    Matrix rotationJacobian;
    const Point rotated =
        Ri.rotate(measured_, H1 ? &rotationJacobian : nullptr);
 
    if (H1) *H1 = -sw * rotationJacobian;
    if (H2) *H2 = -sw * Matrix::Identity(d, d);
    if (H3) *H3 = sw * Matrix::Identity(d, d);
    return sw * (tj - ti - rotated);
  }
 
  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(
            new RelativeTranslationFactor(*this)));
  }

  void qcqpFactors(NonlinearFactorGraph* costs,
                   NonlinearEqualityConstraints* constraints,
                   size_t columnDimension = 1) const override {
    if (columnDimension == 0) {
      throw std::invalid_argument(
          "RelativeTranslationFactor::qcqpFactors requires a positive "
          "columnDimension.");
    }
    if (columnDimension == 1) {
      qcqpFactorsForVector(costs, constraints);
      return;
    }
    if (columnDimension < static_cast<size_t>(d)) {
      throw std::invalid_argument(
          "RelativeTranslationFactor::qcqpFactors requires columnDimension "
          ">= d.");
    }
    if (!costs) {
      throw std::invalid_argument(
          "RelativeTranslationFactor::qcqpFactors: costs is null.");
    }
 
    const double sw = std::sqrt(weight_);
    Matrix B = Matrix::Zero(1, d + 2);
    B.block(0, 0, 1, d) = -sw * measured_.transpose();  // rotation row
    B(0, d) = -sw;                                      // pose i translation
    B(0, d + 1) = sw;                                   // pose j translation
 
    const Matrix Q = B.transpose() * B;
    const SymmetricBlockMatrix blockQ(std::vector<DenseIndex>{d, 1, 1}, 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(
          "RelativeTranslationFactor::qcqpFactors: costs is null.");
    }
 
    constexpr int kRotationDim = traits<Rot>::QcqpVectorDim;
    constexpr int kPointDim = traits<Point>::QcqpVectorDim;
    const double sw = std::sqrt(weight_);
 
    // Express R*measured in the non-homogeneous rotation coordinates.
    Matrix rotateMeasurement;
    if constexpr (d == 2) {
      // [[mx,-my],[my,mx]] [c,s]' = R(c,s)*measured.
      rotateMeasurement.resize(2, 2);
      rotateMeasurement << measured_(0), -measured_(1), measured_(1),
          measured_(0);
    } else {
      // (measured' kron I) vec(R), in column-major order.
      rotateMeasurement = Matrix::Zero(d, d * d);
      for (int column = 0; column < d; ++column) {
        rotateMeasurement.block(0, column * d, d, d) =
            measured_(column) * Matrix::Identity(d, d);
      }
    }
 
    Matrix B = Matrix::Zero(d, kRotationDim + 2 * kPointDim);
    B.block(0, 1, d, kRotationDim - 1) = -sw * rotateMeasurement;
    B.block(0, kRotationDim + 1, d, d) = -sw * Matrix::Identity(d, d);
    B.block(0, kRotationDim + kPointDim + 1, d, d) =
        sw * Matrix::Identity(d, d);
 
    InsertQcqpConstraints<Rot, 1>(this->key1(), constraints);
    InsertQcqpConstraints<Point, 1>(this->key2(), constraints);
    InsertQcqpConstraints<Point, 1>(this->key3(), constraints);
 
    const Matrix Q = B.transpose() * B;
    const SymmetricBlockMatrix blockQ(
        std::vector<DenseIndex>{kRotationDim, kPointDim, kPointDim}, Q);
    costs->push_back(std::make_shared<QpCost>(
        KeyVector{this->key1(), this->key2(), this->key3()}, blockQ));
  }
};
 
using RelativeTranslationFactor2 = RelativeTranslationFactor<2>;
using RelativeTranslationFactor3 = RelativeTranslationFactor<3>;
 
}  // namespace gtsam