TransferFactor.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 * GTSAM Copyright 2010-2024, 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 TransferFactor.h
 * @brief TransferFactor class
 * @author Frank Dellaert
 * @date October 24, 2024
 */
 
#pragma once
 
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/FundamentalMatrix.h>
#include <gtsam/inference/EdgeKey.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
 
namespace gtsam {
 
template <typename F>
class TransferFactor : public NoiseModelFactorN<F, F> {
  EdgeKey key1_, key2_;  
  std::vector<std::tuple<Point2, Point2, Point2>>
      triplets_;  
 
 public:
  TransferFactor(EdgeKey key1, EdgeKey key2, const Point2& pa, const Point2& pb,
                 const Point2& pc, const SharedNoiseModel& model = nullptr)
      : NoiseModelFactorN<F, F>(model, key1, key2),
        key1_(key1),
        key2_(key2),
        triplets_({std::make_tuple(pa, pb, pc)}) {}
 
  TransferFactor(
      EdgeKey key1, EdgeKey key2,
      const std::vector<std::tuple<Point2, Point2, Point2>>& triplets,
      const SharedNoiseModel& model = nullptr)
      : NoiseModelFactorN<F, F>(model, key1, key2),
        key1_(key1),
        key2_(key2),
        triplets_(triplets) {}
 
  // Create Matrix3 objects based on EdgeKey configurations
  std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const {
    // Fill Fca and Fcb based on EdgeKey configurations
    if (key1_.i() == key2_.i()) {
      return {F1.matrix(), F2.matrix()};
    } else if (key1_.i() == key2_.j()) {
      return {F1.matrix(), F2.matrix().transpose()};
    } else if (key1_.j() == key2_.i()) {
      return {F1.matrix().transpose(), F2.matrix()};
    } else if (key1_.j() == key2_.j()) {
      return {F1.matrix().transpose(), F2.matrix().transpose()};
    } else {
      throw std::runtime_error(
          "TransferFactor: invalid EdgeKey configuration.");
    }
  }
 
  Vector evaluateError(const F& F1, const F& F2,
                       OptionalMatrixType H1 = nullptr,
                       OptionalMatrixType H2 = nullptr) const override {
    std::function<Vector(const F&, const F&)> transfer = [&](const F& F1,
                                                             const F& F2) {
      Vector errors(2 * triplets_.size());
      size_t idx = 0;
      auto [Fca, Fcb] = getMatrices(F1, F2);
      for (const auto& tuple : triplets_) {
        const auto& [pa, pb, pc] = tuple;
        Point2 transferredPoint = EpipolarTransfer(Fca, pa, Fcb, pb);
        Vector2 error = transferredPoint - pc;
        errors.segment<2>(idx) = error;
        idx += 2;
      }
      return errors;
    };
    if (H1) *H1 = numericalDerivative21<Vector, F, F>(transfer, F1, F2);
    if (H2) *H2 = numericalDerivative22<Vector, F, F>(transfer, F1, F2);
    return transfer(F1, F2);
  }
};
 
}  // namespace gtsam