FrobeniusFactor.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010-2019, 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/constrained/QcqpProblem.h>
#include <gtsam/constrained/QpCost.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SOn.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
 
#include <memory>
#include <stdexcept>
#include <type_traits>
#include <vector>
 
namespace gtsam {

GTSAM_EXPORT SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel& model,
                                                size_t n,
                                                bool defaultToUnit = true);

template <class T, size_t Dim>
inline SharedNoiseModel ConvertModel(const SharedNoiseModel& model) {
  if (!model) {
    return ConvertNoiseModel(noiseModel::Unit::Create(T()), Dim);
  }
  if (model->dim() == Dim) {
    return model;
  }
  if (model->dim() != T::dimension) {
    throw std::runtime_error(
        "Noise model dimension does not match expected dimension for T.");
  }
  return ConvertNoiseModel(model, Dim);
}
 
template <class T>
using FrobeniusErrorVector = Eigen::Matrix<
    double, T::LieAlgebra::RowsAtCompileTime * T::LieAlgebra::RowsAtCompileTime,
    1>;

template <class T>
class FrobeniusPrior : public NoiseModelFactorN<T> {
  GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<T>);
  inline constexpr static auto N = T::LieAlgebra::RowsAtCompileTime;
  inline constexpr static auto Dim = N * N;
 
 public:
  using MatrixN = Eigen::Matrix<double, N, N>;
 
 private:
  Eigen::Matrix<double, Dim, 1> vecM_;  
 
 public:
  // Provide access to the Matrix& version of evaluateError:
  using NoiseModelFactor1<T>::evaluateError;

  FrobeniusPrior(Key j, const MatrixN& M,
                 const SharedNoiseModel& model = nullptr)
      : NoiseModelFactorN<T>(ConvertModel<T, Dim>(model), j) {
    vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
  }

  MatrixN priorMatrix() const {
    return Eigen::Map<const MatrixN>(vecM_.data());
  }

  Vector evaluateError(const T& g, OptionalMatrixType H) const override {
    return traits<T>::Vec(g, H) -
           vecM_;  // Jacobian is computed only when needed.
  }

  void qcqpFactors(NonlinearFactorGraph* costs,
                   NonlinearEqualityConstraints* constraints,
                   size_t columnDimension = 1) const override {
    if (columnDimension == 0) {
      throw std::invalid_argument(
          "FrobeniusPrior::qcqpFactors: columnDimension must be >= 1");
    }
    if (columnDimension == 1) {
      qcqpFactorsForVec(costs, constraints);
    } else {
      qcqpFactorsForMatrix(costs, constraints, columnDimension);
    }
  }
 
 private:
  void qcqpFactorsForVec(NonlinearFactorGraph* costs,
                         NonlinearEqualityConstraints* constraints) const {
    if constexpr (!internal::HasQcqpVariableTraits<T, 1>::value) {
      (void)costs;
      (void)constraints;
      throw std::runtime_error(
          "FrobeniusPrior::qcqpFactors requires QCQP variable traits for this "
          "type and column dimension 1.");
    } else if constexpr (!(std::is_same_v<T, Rot2> || std::is_same_v<T, Rot3> ||
                           std::is_same_v<T, Pose2> ||
                           std::is_same_v<T, Pose3>)) {
      (void)costs;
      (void)constraints;
      throw std::runtime_error(
          "FrobeniusPrior::qcqpFactors D=1 is implemented only for Rot2, "
          "Rot3, Pose2, and Pose3.");
    } else {
      (void)costs;
      if (this->noiseModel_->isConstrained()) {
        InsertQcqpConstraints<T, 1>(this->key(), constraints);
 
        constexpr int LiftedDim = traits<T>::QcqpVectorDim;
        Vector target = Vector::Zero(LiftedDim);
        target(0) = 1.0;
        if constexpr (std::is_same_v<T, Rot2>) {
          // vec(R)=[c,s,-s,c], so the compact lift retains its first column.
          target.segment<2>(1) = vecM_.template head<2>();
        } else {
          static_assert((LiftedDim - 1) % N == 0);
          constexpr int M = (LiftedDim - 1) / N;
          // Build the homogeneous target while omitting fixed pose-matrix rows.
          for (int column = 0; column < N; ++column) {
            target.segment<M>(1 + column * M) =
                vecM_.template segment<M>(column * N);
          }
        }
 
        constraints->push_back(
            LinearConstraint::Equal(
                JacobianFactor(this->key(),
                               Matrix::Identity(LiftedDim, LiftedDim), target))
                .createEqualityFactor());
      } else {
        throw std::runtime_error(
            "FrobeniusPrior::qcqpFactors D=1 non-constrained noise is not yet "
            "implemented.");
      }
    }
  }

  void qcqpFactorsForMatrix(NonlinearFactorGraph* costs,
                            NonlinearEqualityConstraints* constraints,
                            size_t columnDimension) const {
    (void)costs;
    (void)constraints;
    (void)columnDimension;
    throw std::runtime_error(
        "FrobeniusPrior::qcqpFactors does not support matrix-form priors; "
        "a Burer--Monteiro-compatible prior requires an anchor block.");
  }
};

template <class T>
class FrobeniusFactor
    : public NoiseModelFactorT<FrobeniusErrorVector<T>, T, T> {
  GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<T>);
  inline constexpr static auto N = T::LieAlgebra::RowsAtCompileTime;
  inline constexpr static auto Dim = N * N;
  using Base = NoiseModelFactorT<FrobeniusErrorVector<T>, T, T>;
  using VectorD = FrobeniusErrorVector<T>;
 
 public:
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
      : Base(ConvertModel<T, Dim>(model), j1, j2) {}

  VectorD evaluateError(const T& T1, const T& T2, OptionalMatrixType H1,
                        OptionalMatrixType H2) const override {
    VectorD error = traits<T>::Vec(T2, H2) - traits<T>::Vec(T1, H1);
    if (H1) *H1 = -*H1;
    return error;
  }
};

template <class T>
class FrobeniusBetweenFactorNL
    : public NoiseModelFactorT<FrobeniusErrorVector<T>, T, T> {
  GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<T>);
  inline constexpr static auto N = T::LieAlgebra::RowsAtCompileTime;
  inline constexpr static auto Dim = N * N;
  static_assert(N > 0, "The Lie algebra dimension N must be greater than 0.");
 
 protected:
  T T12_;  
 
  using MatrixN = Eigen::Matrix<double, N, N>;
  using VectorD = Eigen::Matrix<double, Dim, 1>;
  using Base = NoiseModelFactorT<VectorD, T, T>;
 
 public:
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;


  FrobeniusBetweenFactorNL(Key j1, Key j2, const T& T12,
                           const SharedNoiseModel& model = nullptr)
      : Base(ConvertModel<T, Dim>(model), j1, j2), T12_(T12) {}

  const T& measured() const { return T12_; }


  void print(const std::string& s, const KeyFormatter& keyFormatter =
                                       DefaultKeyFormatter) const override {
    std::cout << s << "FrobeniusBetweenFactorNL<" << demangle(typeid(T).name())
              << ">(" << keyFormatter(this->key1()) << ","
              << keyFormatter(this->key2()) << ")\n";
    traits<T>::Print(T12_, "  T12: ");
    this->noiseModel_->print("  noise model: ");
  }

  bool equals(const NonlinearFactor& expected,
              double tol = 1e-9) const override {
    const auto* e = dynamic_cast<const FrobeniusBetweenFactorNL*>(&expected);
    return e != nullptr && Base::equals(*e, tol) &&
           traits<T>::Equals(this->T12_, e->T12_, tol);
  }


  VectorD evaluateError(const T& T1, const T& T2, OptionalMatrixType H1,
                        OptionalMatrixType H2) const override {
    const bool computeJacobians = H1 || H2;
 
    // Compute the predicted inverse-relative transform inv(T2) * T1.
    typename T::Jacobian H_T21_T2;
    const T hatT21 =
        traits<T>::Between(T2, T1, computeJacobians ? &H_T21_T2 : nullptr);
 
    // Compose with the measurement; the result should be identity.
    typename T::Jacobian H_pred_hat = T::Jacobian::Zero();
    const T pred = traits<T>::Compose(hatT21, T12_,
                                      computeJacobians ? &H_pred_hat : nullptr);
 
    // Cache the fixed-size vectorization of the identity matrix.
    static const VectorD vecI = [] {
      const MatrixN I = MatrixN::Identity();
      return VectorD(Eigen::Map<const VectorD>(I.data()));
    }();
 
    // Vectorize the residual and retain its derivative for the chain rule.
    Eigen::Matrix<double, Dim, T::dimension> H_vec_pred;
    VectorD error =
        traits<T>::Vec(pred, computeJacobians ? &H_vec_pred : nullptr) - vecI;
 
    // Propagate derivatives through Between and Compose.
    if (computeJacobians) {
      const auto H_error_hat21 = H_vec_pred * H_pred_hat;
      if (H1) *H1 = H_error_hat21;  // H_pred_T1 is identity
      if (H2) *H2 = H_error_hat21 * H_T21_T2;
    }
    return error;
  }

};

template <class T>
class FrobeniusBetweenFactor : public FrobeniusBetweenFactorNL<T> {
  inline constexpr static auto N = T::LieAlgebra::RowsAtCompileTime;
  inline constexpr static auto Dim = N * N;
  using Base = FrobeniusBetweenFactorNL<T>;
  using MatrixN = typename Base::MatrixN;
  using VectorD = FrobeniusErrorVector<T>;
 
  typename T::Jacobian T2hat_H_T1_;  
 
 public:
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  FrobeniusBetweenFactor(Key j1, Key j2, const T& T12,
                         const SharedNoiseModel& model = nullptr)
      : FrobeniusBetweenFactorNL<T>(j1, j2, T12, model),
        T2hat_H_T1_(traits<T>::AdjointMap(traits<T>::Inverse(T12))) {}

  VectorD evaluateError(const T& T1, const T& T2, OptionalMatrixType H1,
                        OptionalMatrixType H2) const override {
    const T T2hat = traits<T>::Compose(T1, this->T12_);
    Eigen::Matrix<double, Dim, T::dimension> vec_H_T2hat;
    VectorD error = traits<T>::Vec(T2, H2) -
                    traits<T>::Vec(T2hat, H1 ? &vec_H_T2hat : nullptr);
    if (H1) *H1 = -vec_H_T2hat * T2hat_H_T1_;
    return error;
  }

  void qcqpFactors(NonlinearFactorGraph* costs,
                   NonlinearEqualityConstraints* constraints,
                   size_t columnDimension = 1) const override {
    if (columnDimension == 0) {
      throw std::invalid_argument(
          "FrobeniusBetweenFactor::qcqpFactors: columnDimension must be >= 1");
    }
    if (columnDimension == 1) {
      qcqpFactorsForVec(costs, constraints);
    } else {
      qcqpFactorsForMatrix(costs, constraints, columnDimension);
    }
  }
 
 private:
  static Matrix internalKroneckerProduct(const Matrix& left,
                                         const Matrix& right) {
    const DenseIndex resultRows = left.rows() * right.rows();
    const DenseIndex resultColumns = left.cols() * right.cols();
    Matrix result = Matrix::Zero(resultRows, resultColumns);
 
    for (DenseIndex row = 0; row < left.rows(); ++row) {
      for (DenseIndex column = 0; column < left.cols(); ++column) {
        result.block(row * right.rows(), column * right.cols(), right.rows(),
                     right.cols()) = left(row, column) * right;
      }
    }
    return result;
  }

  void qcqpFactorsForVec(NonlinearFactorGraph* costs,
                         NonlinearEqualityConstraints* constraints) const {
    if constexpr (!internal::HasQcqpVariableTraits<T, 1>::value) {
      (void)costs;
      (void)constraints;
      throw std::runtime_error(
          "FrobeniusBetweenFactor::qcqpFactors requires QCQP variable traits "
          "for this type and column dimension 1.");
    } else if constexpr (!(std::is_same_v<T, Rot2> || std::is_same_v<T, Rot3> ||
                           std::is_same_v<T, Pose2> ||
                           std::is_same_v<T, Pose3>)) {
      (void)costs;
      (void)constraints;
      throw std::runtime_error(
          "FrobeniusBetweenFactor::qcqpFactors D=1 is implemented only for "
          "Rot2, Rot3, Pose2, and Pose3.");
    } else {
      if (!costs) {
        throw std::invalid_argument(
            "FrobeniusBetweenFactor::qcqpFactors costs is null");
      }
      if (std::dynamic_pointer_cast<noiseModel::Robust>(this->noiseModel_) ||
          this->noiseModel_->isConstrained()) {
        throw std::runtime_error(
            "FrobeniusBetweenFactor::qcqpFactors requires a "
            "non-robust/non-hard quadratic noise model");
      }
 
      constexpr int LiftedDim = traits<T>::QcqpVectorDim;
      Matrix Q_trunc_hom;
      if constexpr (std::is_same_v<T, Rot2>) {
        // vec(R)=L[c,s]' exactly. Since SO(2) is abelian,
        // vec(R2-R1*M)=L(q2-M*q1).
        Matrix L(4, 2);
        L << 1.0, 0.0, 0.0, 1.0, 0.0, -1.0, 1.0, 0.0;
        Matrix fullB = Matrix::Zero(4, 2 * LiftedDim);
        fullB.block<4, 2>(0, 1) = -L * this->T12_.matrix();
        fullB.block<4, 2>(0, LiftedDim + 1) = L;
        const Matrix whitenedB = this->noiseModel_->Whiten(fullB);
        Q_trunc_hom = whitenedB.transpose() * whitenedB;
      } else {
        constexpr int MN = LiftedDim - 1;
        static_assert(MN % N == 0);
        constexpr int M = MN / N;
        using MatrixM = Eigen::Matrix<double, M, M>;
 
        const MatrixN measurement = this->T12_.matrix();
        const MatrixM I_M = MatrixM::Identity();
        // Column-major vec(XM) = (M.transpose() kron I) vec(X).
        const Matrix A = internalKroneckerProduct(measurement.transpose(), I_M);
 
        Matrix B = Matrix::Zero(MN, 2 * MN);
        B.block<MN, MN>(0, 0) = -A;
        B.block<MN, MN>(0, MN).setIdentity();
 
        Matrix fullB = Matrix::Zero(Dim, 2 * MN);
        for (int column = 0; column < N; ++column) {
          fullB.block<M, 2 * MN>(column * N, 0) =
              B.block<M, 2 * MN>(column * M, 0);
        }
 
        const Matrix whitenedB = this->noiseModel_->Whiten(fullB);
        const Matrix Q = whitenedB.transpose() * whitenedB;
        Q_trunc_hom = Matrix::Zero(2 * LiftedDim, 2 * LiftedDim);
        Q_trunc_hom.block<MN, MN>(1, 1) = Q.block<MN, MN>(0, 0);
        Q_trunc_hom.block<MN, MN>(1, LiftedDim + 1) = Q.block<MN, MN>(0, MN);
        Q_trunc_hom.block<MN, MN>(LiftedDim + 1, 1) = Q.block<MN, MN>(MN, 0);
        Q_trunc_hom.block<MN, MN>(LiftedDim + 1, LiftedDim + 1) =
            Q.block<MN, MN>(MN, MN);
      }
 
      InsertQcqpConstraints<T, 1>(this->key1(), constraints);
      InsertQcqpConstraints<T, 1>(this->key2(), constraints);
 
      const SymmetricBlockMatrix blockQ(
          std::vector<DenseIndex>{LiftedDim, LiftedDim}, Q_trunc_hom);
      costs->push_back(std::make_shared<QpCost>(
          KeyVector{this->key1(), this->key2()}, blockQ));
    }
  }

  void qcqpFactorsForMatrix(NonlinearFactorGraph* costs,
                            NonlinearEqualityConstraints* constraints,
                            size_t columnDimension) const {
    if constexpr (!internal::HasQcqpVariableTraits<T, N>::value) {
      (void)costs;
      (void)constraints;
      (void)columnDimension;
      throw std::runtime_error(
          "FrobeniusBetweenFactor::qcqpFactors requires QCQP variable traits "
          "for this type and matrix-form column dimensions (>= 2).");
    } else {
      if (columnDimension < static_cast<size_t>(N)) {
        throw std::invalid_argument(
            "FrobeniusBetweenFactor::qcqpFactors: columnDimension must be "
            ">= the variable's intrinsic row dimension (e.g. >= 3 for Rot3).");
      }
      if (!costs) {
        throw std::invalid_argument(
            "FrobeniusBetweenFactor::qcqpFactors costs is null");
      }
      if (std::dynamic_pointer_cast<noiseModel::Robust>(this->noiseModel_) ||
          this->noiseModel_->isConstrained()) {
        throw std::runtime_error(
            "FrobeniusBetweenFactor::qcqpFactors requires a "
            "non-robust quadratic noise model");
      }
      const auto isotropic =
          std::dynamic_pointer_cast<noiseModel::Isotropic>(this->noiseModel_);
      if (!isotropic) {
        throw std::runtime_error(
            "FrobeniusBetweenFactor::qcqpFactors with column dimension > 1 "
            "requires an isotropic noise model");
      }
 
      InsertQcqpConstraints<T, N>(this->key1(), constraints);
      InsertQcqpConstraints<T, N>(this->key2(), constraints);
 
      const MatrixN measurement = this->T12_.matrix();
      const MatrixN I = MatrixN::Identity();
      const double weight = 1.0 / (isotropic->sigma() * isotropic->sigma());
 
      // Build B = [-M' I] and its isotropically weighted Hessian.
      Matrix B = Matrix::Zero(N, 2 * N);
      B.block<N, N>(0, 0) = -measurement.transpose();
      B.block<N, N>(0, N) = I;
 
      const Matrix Q = weight * B.transpose() * B;
      const SymmetricBlockMatrix blockQ(std::vector<DenseIndex>{N, N}, Q);
      costs->push_back(std::make_shared<QpCost>(
          KeyVector{this->key1(), this->key2()}, blockQ, columnDimension));
    }
  }
};

template <class T>
class FrobeniusLeftBetweenFactor : public FrobeniusBetweenFactorNL<T> {
  inline constexpr static auto N = T::LieAlgebra::RowsAtCompileTime;
  inline constexpr static auto Dim = N * N;
  using Base = FrobeniusBetweenFactorNL<T>;
  using This = FrobeniusLeftBetweenFactor<T>;
  using MatrixN = typename Base::MatrixN;
  using VectorD = FrobeniusErrorVector<T>;
 
 public:
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;

  FrobeniusLeftBetweenFactor(Key j1, Key j2, const T& iTj,
                             const SharedNoiseModel& model = nullptr)
      : Base(j1, j2, iTj, ConvertLeftModel(model)) {}
 
  NonlinearFactor::shared_ptr clone() const override {
    return std::make_shared<This>(*this);
  }

  void print(const std::string& s, const KeyFormatter& keyFormatter =
                                       DefaultKeyFormatter) const override {
    std::cout << s << "FrobeniusLeftBetweenFactor<"
              << demangle(typeid(T).name()) << ">("
              << keyFormatter(this->key1()) << "," << keyFormatter(this->key2())
              << ")\n";
    traits<T>::Print(this->T12_, "  measured iTj: ");
    this->noiseModel_->print("  noise model: ");
  }

  bool equals(const NonlinearFactor& expected,
              double tol = 1e-9) const override {
    const auto* e = dynamic_cast<const FrobeniusLeftBetweenFactor*>(&expected);
    return e != nullptr && Base::equals(*e, tol);
  }

  VectorD evaluateError(const T& iTw, const T& jTw, OptionalMatrixType H1,
                        OptionalMatrixType H2) const override {
    typename T::Jacobian predicted_H_jTw;
    const T predicted_iTw = traits<T>::Compose(this->T12_, jTw, {},
                                               H2 ? &predicted_H_jTw : nullptr);
 
    Eigen::Matrix<double, Dim, T::dimension> vec_H_predicted;
    VectorD error =
        traits<T>::Vec(iTw, H1) -
        traits<T>::Vec(predicted_iTw, H2 ? &vec_H_predicted : nullptr);
    if (H2) *H2 = -vec_H_predicted * predicted_H_jTw;
    return error;
  }

  void qcqpFactors(NonlinearFactorGraph* costs,
                   NonlinearEqualityConstraints* constraints,
                   size_t columnDimension = 1) const override {
    if (columnDimension != 1) {
      throw std::invalid_argument(
          "FrobeniusLeftBetweenFactor::qcqpFactors only supports column "
          "dimension 1");
    }
    qcqpFactorsForVec(costs, constraints);
  }
 
 private:
  static SharedNoiseModel ConvertLeftModel(const SharedNoiseModel& model) {
    if (!model || model->dim() != T::dimension) return model;
 
    try {
      return ConvertNoiseModel(model, Dim, false);
    } catch (const std::runtime_error&) {
      throw std::invalid_argument(
          "FrobeniusLeftBetweenFactor cannot convert an anisotropic "
          "manifold-dimensional noise model to the ambient residual");
    }
  }

  static Matrix internalKroneckerProduct(const Matrix& left,
                                         const Matrix& right) {
    const DenseIndex resultRows = left.rows() * right.rows();
    const DenseIndex resultColumns = left.cols() * right.cols();
    Matrix result = Matrix::Zero(resultRows, resultColumns);
 
    for (DenseIndex row = 0; row < left.rows(); ++row) {
      for (DenseIndex column = 0; column < left.cols(); ++column) {
        result.block(row * right.rows(), column * right.cols(), right.rows(),
                     right.cols()) = left(row, column) * right;
      }
    }
    return result;
  }

  void qcqpFactorsForVec(NonlinearFactorGraph* costs,
                         NonlinearEqualityConstraints* constraints) const {
    if constexpr (!internal::HasQcqpVariableTraits<T, 1>::value) {
      (void)costs;
      (void)constraints;
      throw std::runtime_error(
          "FrobeniusLeftBetweenFactor::qcqpFactors requires QCQP variable "
          "traits for this type and column dimension 1.");
    } else if constexpr (!(std::is_same_v<T, Rot2> || std::is_same_v<T, Rot3> ||
                           std::is_same_v<T, Pose2> ||
                           std::is_same_v<T, Pose3>)) {
      (void)costs;
      (void)constraints;
      throw std::runtime_error(
          "FrobeniusLeftBetweenFactor::qcqpFactors D=1 is implemented only "
          "for Rot2, Rot3, Pose2, and Pose3.");
    } else {
      if (!costs) {
        throw std::invalid_argument(
            "FrobeniusLeftBetweenFactor::qcqpFactors costs is null");
      }
      if (std::dynamic_pointer_cast<noiseModel::Robust>(this->noiseModel_) ||
          this->noiseModel_->isConstrained()) {
        throw std::runtime_error(
            "FrobeniusLeftBetweenFactor::qcqpFactors requires a "
            "non-robust/non-hard quadratic noise model");
      }
 
      constexpr int LiftedDim = traits<T>::QcqpVectorDim;
      Matrix Q;
      if constexpr (std::is_same_v<T, Rot2>) {
        Matrix L(4, 2);
        L << 1.0, 0.0, 0.0, 1.0, 0.0, -1.0, 1.0, 0.0;
        Matrix fullB = Matrix::Zero(4, 2 * LiftedDim);
        fullB.block<4, 2>(0, 1) = L;
        fullB.block<4, 2>(0, LiftedDim + 1) = -L * this->T12_.matrix();
        const Matrix whitenedB = this->noiseModel_->Whiten(fullB);
        Q = whitenedB.transpose() * whitenedB;
      } else {
        constexpr int MN = LiftedDim - 1;
        static_assert(MN % N == 0);
        constexpr int M = MN / N;
        using MatrixM = Eigen::Matrix<double, M, M>;
 
        const MatrixN iTj = this->T12_.matrix();
        const MatrixM linearPart = iTj.template topLeftCorner<M, M>();
        const Matrix leftAction =
            internalKroneckerProduct(MatrixN::Identity(), linearPart);
 
        Vector offset = Vector::Zero(MN);
        if constexpr (M < N) {
          offset.segment((N - 1) * M, M) = iTj.block(0, N - 1, M, 1);
        }
 
        Matrix retainedB = Matrix::Zero(MN, 2 * LiftedDim);
        retainedB.block(0, 1, MN, MN).setIdentity();
        retainedB.col(LiftedDim) = -offset;
        retainedB.block(0, LiftedDim + 1, MN, MN) = -leftAction;
 
        Matrix fullB = Matrix::Zero(Dim, 2 * LiftedDim);
        for (int column = 0; column < N; ++column) {
          fullB.block(column * N, 0, M, 2 * LiftedDim) =
              retainedB.block(column * M, 0, M, 2 * LiftedDim);
        }
 
        const Matrix whitenedB = this->noiseModel_->Whiten(fullB);
        Q = whitenedB.transpose() * whitenedB;
      }
 
      InsertQcqpConstraints<T, 1>(this->key1(), constraints);
      InsertQcqpConstraints<T, 1>(this->key2(), constraints);
 
      const SymmetricBlockMatrix blockQ(
          std::vector<DenseIndex>{LiftedDim, LiftedDim}, Q);
      costs->push_back(std::make_shared<QpCost>(
          KeyVector{this->key1(), this->key2()}, blockQ));
    }
  }
};
 
}  // namespace gtsam