TransferFactor.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2  * GTSAM Copyright 2010-2024, Georgia Tech Research Corporation,
3  * Atlanta, Georgia 30332-0415
4  * All Rights Reserved
5  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
6  * See LICENSE for the license information
7  * -------------------------------------------------------------------------- */
8 
9 /*
10  * @file TransferFactor.h
11  * @brief TransferFactor class
12  * @author Frank Dellaert
13  * @date October 24, 2024
14  */
15 
16 #pragma once
17 
22 #include <gtsam/inference/Symbol.h>
24 
25 #include <cstdint>
26 
27 namespace gtsam {
28 
32 template <typename F>
34  protected:
37 
38  public:
40  : edge1_(edge1), edge2_(edge2), c_(ViewC(edge1, edge2)) {}
41 
43  static size_t ViewA(const EdgeKey& edge1, const EdgeKey& edge2) {
44  size_t c = ViewC(edge1, edge2);
45  return (edge1.i() == c) ? edge1.j() : edge1.i();
46  }
47 
49  static size_t ViewB(const EdgeKey& edge1, const EdgeKey& edge2) {
50  size_t c = ViewC(edge1, edge2);
51  return (edge2.i() == c) ? edge2.j() : edge2.i();
52  }
53 
55  static size_t ViewC(const EdgeKey& edge1, const EdgeKey& edge2) {
56  if (edge1.i() == edge2.i() || edge1.i() == edge2.j())
57  return edge1.i();
58  else if (edge1.j() == edge2.i() || edge1.j() == edge2.j())
59  return edge1.j();
60  else
61  throw std::runtime_error(
62  "EssentialTransferFactorK: No common key in edge keys.");
63  }
64 
66  std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const {
67  // Determine whether to transpose F1
68  const Matrix3 Fca =
69  edge1_.i() == c_ ? F1.matrix() : F1.matrix().transpose();
70 
71  // Determine whether to transpose F2
72  const Matrix3 Fcb =
73  edge2_.i() == c_ ? F2.matrix() : F2.matrix().transpose();
74 
75  return {Fca, Fcb};
76  }
77 };
78 
86 template <typename F>
87 class TransferFactor : public NoiseModelFactorN<F, F>, public TransferEdges<F> {
88  public:
90  using Triplet = std::tuple<Point2, Point2, Point2>;
91 
92  protected:
93  std::vector<Triplet> triplets_;
94 
95  public:
106  const std::vector<Triplet>& triplets,
107  const SharedNoiseModel& model = nullptr)
108  : Base(model, edge1, edge2),
109  TransferEdges<F>(edge1, edge2),
110  triplets_(triplets) {}
111 
113  Vector evaluateError(const F& F1, const F& F2,
114  OptionalMatrixType H1 = nullptr,
115  OptionalMatrixType H2 = nullptr) const override {
116  std::function<Vector(const F&, const F&)> errorFunction = [&](const F& f1,
117  const F& f2) {
118  Vector errors(2 * triplets_.size());
119  size_t idx = 0;
120  auto [Fca, Fcb] = this->getMatrices(f1, f2);
121  for (const auto& [pa, pb, pc] : triplets_) {
122  errors.segment<2>(idx) = EpipolarTransfer(Fca, pa, Fcb, pb) - pc;
123  idx += 2;
124  }
125  return errors;
126  };
127 
128  if (H1) *H1 = numericalDerivative21(errorFunction, F1, F2);
129  if (H2) *H2 = numericalDerivative22(errorFunction, F1, F2);
130  return errorFunction(F1, F2);
131  }
132 };
133 
145 template <typename K>
146 class EssentialTransferFactor : public TransferFactor<EssentialMatrix> {
148  using Triplet = std::tuple<Point2, Point2, Point2>;
149  std::shared_ptr<K> calibration_;
150 
151  public:
154  using shared_ptr = std::shared_ptr<This>;
155 
167  const std::vector<Triplet>& triplets,
168  const std::shared_ptr<K>& calibration,
169  const SharedNoiseModel& model = nullptr)
170  : Base(edge1, edge2, triplets, model), calibration_(calibration) {}
171 
173  Vector2 TransferError(const Matrix3& Eca, const Point2& pa,
174  const Matrix3& Ecb, const Point2& pb,
175  const Point2& pc) const {
176  const Point2 pA = calibration_->calibrate(pa);
177  const Point2 pB = calibration_->calibrate(pb);
178  const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB);
179  return calibration_->uncalibrate(pC) - pc;
180  }
181 
183  Vector evaluateError(const EM& E1, const EM& E2,
184  OptionalMatrixType H1 = nullptr,
185  OptionalMatrixType H2 = nullptr) const override {
186  std::function<Vector(const EM&, const EM&)> errorFunction =
187  [&](const EM& e1, const EM& e2) {
188  Vector errors(2 * this->triplets_.size());
189  size_t idx = 0;
190  auto [Eca, Ecb] = this->getMatrices(e1, e2);
191  for (const auto& [pa, pb, pc] : this->triplets_) {
192  errors.segment<2>(idx) = TransferError(Eca, pa, Ecb, pb, pc);
193  idx += 2;
194  }
195  return errors;
196  };
197 
198  // Compute error
199  Vector errors = errorFunction(E1, E2);
200 
201  // Compute Jacobians if requested
202  if (H1) *H1 = numericalDerivative21(errorFunction, E1, E2);
203  if (H2) *H2 = numericalDerivative22(errorFunction, E1, E2);
204 
205  return errors;
206  }
207 };
208 
223 template <typename K>
225  : public NoiseModelFactorN<EssentialMatrix, EssentialMatrix, K, K, K>,
226  TransferEdges<EssentialMatrix> {
228  using Triplet = std::tuple<Point2, Point2, Point2>;
229  std::vector<Triplet> triplets_;
230 
231  public:
234  using shared_ptr = std::shared_ptr<This>;
235 
245  const std::vector<Triplet>& triplets,
246  const SharedNoiseModel& model = nullptr)
247  : Base(model, edge1, edge2,
248  Symbol('k', ViewA(edge1, edge2)), // calibration key for view a
249  Symbol('k', ViewB(edge1, edge2)), // calibration key for view b
250  Symbol('k', ViewC(edge1, edge2))), // calibration key for target c
251  TransferEdges<EM>(edge1, edge2),
252  triplets_(triplets) {}
253 
255  Vector2 TransferError(const Matrix3& Eca, const K& Ka, const Point2& pa,
256  const Matrix3& Ecb, const K& Kb, const Point2& pb,
257  const K& Kc, const Point2& pc) const {
258  const Point2 pA = Ka.calibrate(pa);
259  const Point2 pB = Kb.calibrate(pb);
260  const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB);
261  return Kc.uncalibrate(pC) - pc;
262  }
263 
265  Vector evaluateError(const EM& E1, const EM& E2, const K& Ka, const K& Kb,
266  const K& Kc, OptionalMatrixType H1 = nullptr,
267  OptionalMatrixType H2 = nullptr,
268  OptionalMatrixType H3 = nullptr,
269  OptionalMatrixType H4 = nullptr,
270  OptionalMatrixType H5 = nullptr) const override {
271  std::function<Vector(const EM&, const EM&, const K&, const K&, const K&)>
272  errorFunction = [&](const EM& e1, const EM& e2, const K& kA,
273  const K& kB, const K& kC) {
274  Vector errors(2 * triplets_.size());
275  size_t idx = 0;
276  auto [Eca, Ecb] = this->getMatrices(e1, e2);
277  for (const auto& [pa, pb, pc] : triplets_) {
278  errors.segment<2>(idx) =
279  TransferError(Eca, kA, pa, Ecb, kB, pb, kC, pc);
280  idx += 2;
281  }
282  return errors;
283  };
284 
285  // Compute error
286  Vector errors = errorFunction(E1, E2, Ka, Kb, Kc);
287 
288  // Compute Jacobians if requested
289  if (H1) *H1 = numericalDerivative51(errorFunction, E1, E2, Ka, Kb, Kc);
290  if (H2) *H2 = numericalDerivative52(errorFunction, E1, E2, Ka, Kb, Kc);
291  if (H3) *H3 = numericalDerivative53(errorFunction, E1, E2, Ka, Kb, Kc);
292  if (H4) *H4 = numericalDerivative54(errorFunction, E1, E2, Ka, Kb, Kc);
293  if (H5) *H5 = numericalDerivative55(errorFunction, E1, E2, Ka, Kb, Kc);
294 
295  return errors;
296  }
297 
299  size_t dim() const override { return 2 * triplets_.size(); }
300 };
301 
302 } // namespace gtsam
gtsam::TransferEdges
Definition: TransferFactor.h:33
gtsam::TransferFactor::TransferFactor
TransferFactor(EdgeKey edge1, EdgeKey edge2, const std::vector< Triplet > &triplets, const SharedNoiseModel &model=nullptr)
Constructor that accepts a vector of point triplets.
Definition: TransferFactor.h:105
gtsam::numericalDerivative54
internal::FixedSizeMatrix< Y, X4 >::type numericalDerivative54(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Definition: numericalDerivative.h:570
EssentialMatrix.h
test_constructor::f1
auto f1
Definition: testHybridNonlinearFactor.cpp:56
gtsam::TransferFactor< EssentialMatrix >::Triplet
std::tuple< Point2, Point2, Point2 > Triplet
Definition: TransferFactor.h:90
uint32_t
unsigned int uint32_t
Definition: ms_stdint.h:85
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::TransferEdges::ViewB
static size_t ViewB(const EdgeKey &edge1, const EdgeKey &edge2)
Returns the view B index based on the EdgeKeys.
Definition: TransferFactor.h:49
gtsam::EssentialTransferFactorK::Triplet
std::tuple< Point2, Point2, Point2 > Triplet
Definition: TransferFactor.h:228
gtsam::numericalDerivative53
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative53(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Definition: numericalDerivative.h:534
asiaCPTs::pA
ADT pA
Definition: testAlgebraicDecisionTree.cpp:153
gtsam::EssentialTransferFactor::EssentialTransferFactor
EssentialTransferFactor(EdgeKey edge1, EdgeKey edge2, const std::vector< Triplet > &triplets, const std::shared_ptr< K > &calibration, const SharedNoiseModel &model=nullptr)
Constructor that accepts a vector of point triplets and a shared calibration.
Definition: TransferFactor.h:166
E2
E2
Definition: test_numpy_dtypes.cpp:103
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:195
f2
double f2(const Vector2 &x)
Definition: testNumericalDerivative.cpp:56
gtsam::Factor
Definition: Factor.h:70
gtsam::EpipolarTransfer
Point2 EpipolarTransfer(const Matrix3 &Fca, const Point2 &pa, const Matrix3 &Fcb, const Point2 &pb)
Transfer projections from cameras a and b to camera c.
Definition: FundamentalMatrix.cpp:15
gtsam::EssentialTransferFactorK
Transfers points between views using essential matrices, optimizes for calibrations of the views,...
Definition: TransferFactor.h:224
gtsam::Factor::shared_ptr
std::shared_ptr< Factor > shared_ptr
A shared_ptr to this class.
Definition: Factor.h:76
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::TransferEdges::TransferEdges
TransferEdges(EdgeKey edge1, EdgeKey edge2)
Definition: TransferFactor.h:39
gtsam::EssentialTransferFactor
Transfers points between views using essential matrices with a shared calibration.
Definition: TransferFactor.h:146
gtsam::TransferEdges::edge2_
EdgeKey edge2_
The two EdgeKeys.
Definition: TransferFactor.h:35
gtsam::EssentialTransferFactorK::TransferError
Vector2 TransferError(const Matrix3 &Eca, const K &Ka, const Point2 &pa, const Matrix3 &Ecb, const K &Kb, const Point2 &pb, const K &Kc, const Point2 &pc) const
Transfer points pa and pb to view c and evaluate error.
Definition: TransferFactor.h:255
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::TransferEdges::ViewC
static size_t ViewC(const EdgeKey &edge1, const EdgeKey &edge2)
Returns the view C index based on the EdgeKeys.
Definition: TransferFactor.h:55
gtsam::TransferFactor::triplets_
std::vector< Triplet > triplets_
Point triplets.
Definition: TransferFactor.h:93
gtsam::EdgeKey::i
std::uint32_t i() const
Retrieve high 32 bits.
Definition: EdgeKey.h:55
gtsam::EssentialMatrix
Definition: EssentialMatrix.h:26
gtsam::EssentialTransferFactorK::evaluateError
Vector evaluateError(const EM &E1, const EM &E2, const K &Ka, const K &Kb, const K &Kc, OptionalMatrixType H1=nullptr, OptionalMatrixType H2=nullptr, OptionalMatrixType H3=nullptr, OptionalMatrixType H4=nullptr, OptionalMatrixType H5=nullptr) const override
Evaluate error function.
Definition: TransferFactor.h:265
gtsam::EssentialTransferFactorK::dim
size_t dim() const override
Return the dimension of the factor.
Definition: TransferFactor.h:299
gtsam::TransferEdges::ViewA
static size_t ViewA(const EdgeKey &edge1, const EdgeKey &edge2)
Returns the view A index based on the EdgeKeys.
Definition: TransferFactor.h:43
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
gtsam::EssentialTransferFactorK::triplets_
std::vector< Triplet > triplets_
Point triplets.
Definition: TransferFactor.h:229
gtsam::EssentialTransferFactor::evaluateError
Vector evaluateError(const EM &E1, const EM &E2, OptionalMatrixType H1=nullptr, OptionalMatrixType H2=nullptr) const override
Evaluate error function.
Definition: TransferFactor.h:183
Symbol.h
Vector2
Definition: test_operator_overloading.cpp:18
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
gtsam::TransferFactor
Definition: TransferFactor.h:87
F1
const Matrix26 F1
Definition: testRegularImplicitSchurFactor.cpp:37
pc
int RealScalar int RealScalar int RealScalar * pc
Definition: level1_cplx_impl.h:119
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
NonlinearFactor.h
Non-linear factor base classes.
gtsam::EssentialTransferFactorK::EssentialTransferFactorK
EssentialTransferFactorK(EdgeKey edge1, EdgeKey edge2, const std::vector< Triplet > &triplets, const SharedNoiseModel &model=nullptr)
Constructor that accepts a vector of point triplets.
Definition: TransferFactor.h:244
gtsam::EdgeKey
Definition: EdgeKey.h:25
gtsam::EssentialTransferFactor::TransferError
Vector2 TransferError(const Matrix3 &Eca, const Point2 &pa, const Matrix3 &Ecb, const Point2 &pb, const Point2 &pc) const
Transfer points pa and pb to view c and evaluate error.
Definition: TransferFactor.h:173
gtsam
traits
Definition: SFMdata.h:40
K
#define K
Definition: igam.h:8
gtsam::EssentialTransferFactor::Triplet
std::tuple< Point2, Point2, Point2 > Triplet
Definition: TransferFactor.h:148
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:166
FundamentalMatrix.h
gtsam::EdgeKey::j
std::uint32_t j() const
Retrieve low 32 bits.
Definition: EdgeKey.h:58
gtsam::numericalDerivative51
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative51(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Definition: numericalDerivative.h:462
EdgeKey.h
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
gtsam::numericalDerivative52
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative52(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Definition: numericalDerivative.h:498
asiaCPTs::pB
ADT pB
Definition: testAlgebraicDecisionTree.cpp:157
gtsam::numericalDerivative55
internal::FixedSizeMatrix< Y, X5 >::type numericalDerivative55(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Definition: numericalDerivative.h:606
gtsam::EssentialTransferFactor::calibration_
std::shared_ptr< K > calibration_
Shared pointer to calibration object.
Definition: TransferFactor.h:149
gtsam::TransferEdges::c_
uint32_t c_
The transfer target.
Definition: TransferFactor.h:36
gtsam::TransferEdges::getMatrices
std::pair< Matrix3, Matrix3 > getMatrices(const F &F1, const F &F2) const
Create Matrix3 objects based on EdgeKey configurations.
Definition: TransferFactor.h:66
gtsam::TransferFactor::evaluateError
Vector evaluateError(const F &F1, const F &F2, OptionalMatrixType H1=nullptr, OptionalMatrixType H2=nullptr) const override
Vector of errors returns 2*N vector.
Definition: TransferFactor.h:113
E1
E1
Definition: test_numpy_dtypes.cpp:102
gtsam::Symbol
Definition: inference/Symbol.h:37
gtsam::TransferEdges::edge1_
EdgeKey edge1_
Definition: TransferFactor.h:35


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:09:14