EssentialTransferFactor
Frank Dellaert
parent
879bd29963
commit
b33518c4e2
|
|
@ -16,37 +16,63 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/EssentialMatrix.h>
|
||||
#include <gtsam/geometry/FundamentalMatrix.h>
|
||||
#include <gtsam/inference/EdgeKey.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Base class that holds the EdgeKeys and provides the getMatrices method.
|
||||
*/
|
||||
template <typename F>
|
||||
struct TransferEdges {
|
||||
EdgeKey edge1, edge2; ///< The two EdgeKeys.
|
||||
class TransferEdges {
|
||||
protected:
|
||||
EdgeKey edge1_, edge2_; ///< The two EdgeKeys.
|
||||
uint32_t c_; ///< The transfer target
|
||||
|
||||
TransferEdges(EdgeKey edge1, EdgeKey edge2) : edge1(edge1), edge2(edge2) {}
|
||||
public:
|
||||
TransferEdges(EdgeKey edge1, EdgeKey edge2)
|
||||
: edge1_(edge1), edge2_(edge2), c_(viewC(edge1, edge2)) {}
|
||||
|
||||
// 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 (edge1.i() == edge2.i()) {
|
||||
return {F1.matrix(), F2.matrix()};
|
||||
} else if (edge1.i() == edge2.j()) {
|
||||
return {F1.matrix(), F2.matrix().transpose()};
|
||||
} else if (edge1.j() == edge2.i()) {
|
||||
return {F1.matrix().transpose(), F2.matrix()};
|
||||
} else if (edge1.j() == edge2.j()) {
|
||||
return {F1.matrix().transpose(), F2.matrix().transpose()};
|
||||
} else {
|
||||
/// Returns the view A index based on the EdgeKeys
|
||||
static size_t viewA(const EdgeKey& edge1, const EdgeKey& edge2) {
|
||||
size_t c = viewC(edge1, edge2);
|
||||
return (edge1.i() == c) ? edge1.j() : edge1.i();
|
||||
}
|
||||
|
||||
/// Returns the view B index based on the EdgeKeys
|
||||
static size_t viewB(const EdgeKey& edge1, const EdgeKey& edge2) {
|
||||
size_t c = viewC(edge1, edge2);
|
||||
return (edge2.i() == c) ? edge2.j() : edge2.i();
|
||||
}
|
||||
|
||||
/// Returns the view C index based on the EdgeKeys
|
||||
static size_t viewC(const EdgeKey& edge1, const EdgeKey& edge2) {
|
||||
if (edge1.i() == edge2.i() || edge1.i() == edge2.j())
|
||||
return edge1.i();
|
||||
else if (edge1.j() == edge2.i() || edge1.j() == edge2.j())
|
||||
return edge1.j();
|
||||
else
|
||||
throw std::runtime_error(
|
||||
"TransferEdges: invalid EdgeKey configuration between edge1 (" +
|
||||
std::string(edge1) + ") and edge2 (" + std::string(edge2) + ").");
|
||||
}
|
||||
"EssentialTransferFactor: No common key in edge keys.");
|
||||
}
|
||||
|
||||
/// Create Matrix3 objects based on EdgeKey configurations.
|
||||
std::pair<Matrix3, Matrix3> getMatrices(const F& F1, const F& F2) const {
|
||||
// Determine whether to transpose F1
|
||||
const Matrix3 Fca =
|
||||
edge1_.i() == c_ ? F1.matrix() : F1.matrix().transpose();
|
||||
|
||||
// Determine whether to transpose F2
|
||||
const Matrix3 Fcb =
|
||||
edge2_.i() == c_ ? F2.matrix() : F2.matrix().transpose();
|
||||
|
||||
return {Fca, Fcb};
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -64,26 +90,6 @@ class TransferFactor : public NoiseModelFactorN<F, F>, public TransferEdges<F> {
|
|||
std::vector<Triplet> triplets_; ///< Point triplets.
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for a single triplet of points.
|
||||
*
|
||||
* @note Batching all points for the same transfer will be much faster.
|
||||
*
|
||||
* @param edge1 First EdgeKey specifying F1: (a, c) or (c, a).
|
||||
* @param edge2 Second EdgeKey specifying F2: (b, c) or (c, b).
|
||||
* @param pa The point in the first view (a).
|
||||
* @param pb The point in the second view (b).
|
||||
* @param pc The point in the third (and transfer target) view (c).
|
||||
* @param model An optional SharedNoiseModel that defines the noise model
|
||||
* for this factor. Defaults to nullptr.
|
||||
*/
|
||||
TransferFactor(EdgeKey edge1, EdgeKey edge2, const Point2& pa,
|
||||
const Point2& pb, const Point2& pc,
|
||||
const SharedNoiseModel& model = nullptr)
|
||||
: Base(model, edge1, edge2),
|
||||
TransferEdges<F>(edge1, edge2),
|
||||
triplets_({std::make_tuple(pa, pb, pc)}) {}
|
||||
|
||||
/**
|
||||
* @brief Constructor that accepts a vector of point triplets.
|
||||
*
|
||||
|
|
@ -104,25 +110,107 @@ class TransferFactor : public NoiseModelFactorN<F, F>, public TransferEdges<F> {
|
|||
Vector evaluateError(const F& F1, const F& F2,
|
||||
OptionalMatrixType H1 = nullptr,
|
||||
OptionalMatrixType H2 = nullptr) const override {
|
||||
std::function<Vector(const F&, const F&)> errorFunction = [&](const F& F1,
|
||||
const F& F2) {
|
||||
std::function<Vector(const F&, const F&)> errorFunction = [&](const F& f1,
|
||||
const F& f2) {
|
||||
Vector errors(2 * triplets_.size());
|
||||
size_t idx = 0;
|
||||
auto [Fca, Fcb] = this->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;
|
||||
auto [Fca, Fcb] = this->getMatrices(f1, f2);
|
||||
for (const auto& [pa, pb, pc] : triplets_) {
|
||||
errors.segment<2>(idx) = EpipolarTransfer(Fca, pa, Fcb, pb) - pc;
|
||||
idx += 2;
|
||||
}
|
||||
return errors;
|
||||
};
|
||||
if (H1) *H1 = numericalDerivative21<Vector, F, F>(errorFunction, F1, F2);
|
||||
if (H2) *H2 = numericalDerivative22<Vector, F, F>(errorFunction, F1, F2);
|
||||
return errorFunction(F1, F2);
|
||||
|
||||
if (H1) *H1 = numericalDerivative21(errorFunction, F1, F2);
|
||||
if (H2) *H2 = numericalDerivative22(errorFunction, F1, F2);
|
||||
return errorFunction(F1, F2);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @class EssentialTransferFactor
|
||||
* @brief Transfers points between views using essential matrices, optimizes for
|
||||
* calibrations of the views, as well. Note that the EssentialMatrixFactor4 does
|
||||
* something similar but without transfer.
|
||||
*
|
||||
* @note Derives calibration keys from edges, and uses symbol 'k'.
|
||||
*
|
||||
* This factor is templated on the calibration class K and extends
|
||||
* the TransferFactor for EssentialMatrices. It involves two essential matrices
|
||||
* and three calibration objects (Ka, Kb, Kc). The evaluateError
|
||||
* function calibrates the image points, calls the base class's transfer method,
|
||||
* and computes the error using bulk numerical differentiation.
|
||||
*/
|
||||
template <typename K>
|
||||
class EssentialTransferFactor
|
||||
: public NoiseModelFactorN<EssentialMatrix, EssentialMatrix, K, K, K>,
|
||||
TransferEdges<EssentialMatrix> {
|
||||
using EM = EssentialMatrix;
|
||||
using Triplet = std::tuple<Point2, Point2, Point2>;
|
||||
std::vector<Triplet> triplets_; ///< Point triplets
|
||||
|
||||
public:
|
||||
using Base = NoiseModelFactorN<EM, EM, K, K, K>;
|
||||
using This = EssentialTransferFactor<K>;
|
||||
using shared_ptr = std::shared_ptr<This>;
|
||||
|
||||
/**
|
||||
* @brief Constructor that accepts a vector of point triplets.
|
||||
*
|
||||
* @param edge1 First EdgeKey specifying E1: (a, c) or (c, a)
|
||||
* @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b)
|
||||
* @param triplets A vector of triplets containing (pa, pb, pc)
|
||||
* @param model An optional SharedNoiseModel
|
||||
*/
|
||||
EssentialTransferFactor(EdgeKey edge1, EdgeKey edge2,
|
||||
const std::vector<Triplet>& triplets,
|
||||
const SharedNoiseModel& model = nullptr)
|
||||
: Base(model, edge1, edge2,
|
||||
Symbol('k', viewA(edge1, edge2)), // calibration key for view a
|
||||
Symbol('k', viewB(edge1, edge2)), // calibration key for view b
|
||||
Symbol('k', viewC(edge1, edge2))), // calibration key for target c
|
||||
TransferEdges<EM>(edge1, edge2),
|
||||
triplets_(triplets) {}
|
||||
|
||||
/// Evaluate error function
|
||||
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 {
|
||||
std::function<Vector(const EM&, const EM&, const K&, const K&, const K&)>
|
||||
errorFunction = [&](const EM& e1, const EM& e2, const K& kA,
|
||||
const K& kB, const K& kC) {
|
||||
Vector errors(2 * triplets_.size());
|
||||
size_t idx = 0;
|
||||
auto [Eca, Ecb] = this->getMatrices(e1, e2);
|
||||
for (const auto& [pa, pb, pc] : triplets_) {
|
||||
const Point2 pA = kA.calibrate(pa);
|
||||
const Point2 pB = kB.calibrate(pb);
|
||||
const Point2 pC = EpipolarTransfer(Eca, pA, Ecb, pB);
|
||||
errors.segment<2>(idx) = kC.uncalibrate(pC) - pc;
|
||||
idx += 2;
|
||||
}
|
||||
return errors;
|
||||
};
|
||||
|
||||
// Compute error
|
||||
Vector errors = errorFunction(E1, E2, Ka, Kb, Kc);
|
||||
|
||||
// Compute Jacobians if requested
|
||||
if (H1) *H1 = numericalDerivative51(errorFunction, E1, E2, Ka, Kb, Kc);
|
||||
if (H2) *H2 = numericalDerivative52(errorFunction, E1, E2, Ka, Kb, Kc);
|
||||
if (H3) *H3 = numericalDerivative53(errorFunction, E1, E2, Ka, Kb, Kc);
|
||||
if (H4) *H4 = numericalDerivative54(errorFunction, E1, E2, Ka, Kb, Kc);
|
||||
if (H5) *H5 = numericalDerivative55(errorFunction, E1, E2, Ka, Kb, Kc);
|
||||
|
||||
return errors;
|
||||
}
|
||||
|
||||
/// Return the dimension of the factor
|
||||
size_t dim() const override { return 2 * triplets_.size(); }
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,8 +1,8 @@
|
|||
/*
|
||||
* @file testTransferFactor.cpp
|
||||
* @brief Test TransferFactor class
|
||||
* @author Your Name
|
||||
* @date October 23, 2024
|
||||
* @brief Test TransferFactor classes
|
||||
* @author Frank Dellaert
|
||||
* @date October 2024
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -11,9 +11,10 @@
|
|||
#include <gtsam/sfm/TransferFactor.h>
|
||||
|
||||
using namespace gtsam;
|
||||
using symbol_shorthand::K;
|
||||
|
||||
//*************************************************************************
|
||||
/// Generate three cameras on a circle, looking in
|
||||
/// Generate three cameras on a circle, looking inwards
|
||||
std::array<Pose3, 3> generateCameraPoses() {
|
||||
std::array<Pose3, 3> cameraPoses;
|
||||
const double radius = 1.0;
|
||||
|
|
@ -82,9 +83,9 @@ TEST(TransferFactor, Jacobians) {
|
|||
// Create a TransferFactor
|
||||
EdgeKey key01(0, 1), key12(1, 2), key20(2, 0);
|
||||
TransferFactor<SimpleFundamentalMatrix> //
|
||||
factor0{key01, key20, p[1], p[2], p[0]},
|
||||
factor1{key12, key01, p[2], p[0], p[1]},
|
||||
factor2{key20, key12, p[0], p[1], p[2]};
|
||||
factor0{key01, key20, {{p[1], p[2], p[0]}}},
|
||||
factor1{key12, key01, {{p[2], p[0], p[1]}}},
|
||||
factor2{key20, key12, {{p[0], p[1], p[2]}}};
|
||||
|
||||
// Create Values with edge keys
|
||||
Values values;
|
||||
|
|
@ -153,9 +154,68 @@ TEST(TransferFactor, MultipleTuples) {
|
|||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
// Test for EssentialTransferFactor
|
||||
TEST(EssentialTransferFactor, Jacobians) {
|
||||
// Generate cameras on a circle
|
||||
std::array<Pose3, 3> cameraPoses = generateCameraPoses();
|
||||
|
||||
// Create calibration
|
||||
const Cal3_S2 commonK(focalLength, focalLength, 0.0, principalPoint.x(),
|
||||
principalPoint.y());
|
||||
|
||||
// Create cameras
|
||||
std::array<PinholeCamera<Cal3_S2>, 3> cameras;
|
||||
for (size_t i = 0; i < 3; ++i) {
|
||||
cameras[i] = PinholeCamera<Cal3_S2>(cameraPoses[i], commonK);
|
||||
}
|
||||
|
||||
// Create EssentialMatrices between cameras
|
||||
EssentialMatrix E01 =
|
||||
EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[1]));
|
||||
EssentialMatrix E02 =
|
||||
EssentialMatrix::FromPose3(cameraPoses[0].between(cameraPoses[2]));
|
||||
|
||||
// Project a point into the three cameras
|
||||
const Point3 P(0.1, 0.2, 0.3);
|
||||
std::array<Point2, 3> p;
|
||||
for (size_t i = 0; i < 3; ++i) {
|
||||
p[i] = cameras[i].project(P);
|
||||
}
|
||||
|
||||
// Create EdgeKeys
|
||||
EdgeKey key01(0, 1);
|
||||
EdgeKey key02(0, 2);
|
||||
|
||||
// Create an EssentialTransferFactor
|
||||
EssentialTransferFactor<Cal3_S2> factor(key01, key02, {{p[1], p[2], p[0]}});
|
||||
|
||||
// Create Values and insert variables
|
||||
Values values;
|
||||
values.insert(key01, E01); // Essential matrix between views 0 and 1
|
||||
values.insert(key02, E02); // Essential matrix between views 0 and 2
|
||||
values.insert(K(1), commonK); // Calibration for view A (view 1)
|
||||
values.insert(K(2), commonK); // Calibration for view B (view 2)
|
||||
values.insert(K(0), commonK); // Calibration for view C (view 0)
|
||||
|
||||
// Check error
|
||||
Matrix H1, H2, H3, H4, H5;
|
||||
Vector error = factor.evaluateError(E01, E02, commonK, commonK, commonK, //
|
||||
&H1, &H2, &H3, &H4, &H5);
|
||||
EXPECT(H1.rows() == 2 && H1.cols() == 5)
|
||||
EXPECT(H2.rows() == 2 && H2.cols() == 5)
|
||||
EXPECT(H3.rows() == 2 && H3.cols() == 5)
|
||||
EXPECT(H4.rows() == 2 && H4.cols() == 5)
|
||||
EXPECT(H5.rows() == 2 && H5.cols() == 5)
|
||||
EXPECT(assert_equal(Vector::Zero(2), error, 1e-9))
|
||||
|
||||
// Check Jacobians
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-7);
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
//*************************************************************************
|
||||
//*************************************************************************
|
||||
Loading…
Reference in New Issue