Go to the documentation of this file.
44 size_t c =
ViewC(edge1, edge2);
45 return (edge1.
i() ==
c) ? edge1.
j() : edge1.
i();
50 size_t c =
ViewC(edge1, edge2);
51 return (edge2.
i() ==
c) ? edge2.
j() : edge2.
i();
56 if (edge1.
i() == edge2.
i() || edge1.
i() == edge2.
j())
58 else if (edge1.
j() == edge2.
i() || edge1.
j() == edge2.
j())
61 throw std::runtime_error(
62 "EssentialTransferFactorK: No common key in edge keys.");
73 edge2_.
i() ==
c_ ? F2.matrix() : F2.matrix().transpose();
90 using Triplet = std::tuple<Point2, Point2, Point2>;
106 const std::vector<Triplet>& triplets,
116 std::function<
Vector(
const F&,
const F&)> errorFunction = [&](
const F&
f1,
130 return errorFunction(
F1, F2);
145 template <
typename K>
148 using Triplet = std::tuple<Point2, Point2, Point2>;
167 const std::vector<Triplet>& triplets,
168 const std::shared_ptr<K>& calibration,
174 const Matrix3& Ecb,
const Point2& pb,
186 std::function<
Vector(
const EM&,
const EM&)> errorFunction =
187 [&](
const EM& e1,
const EM& e2) {
223 template <
typename K>
228 using Triplet = std::tuple<Point2, Point2, Point2>;
245 const std::vector<Triplet>& triplets,
256 const Matrix3& Ecb,
const K& Kb,
const Point2& pb,
261 return Kc.uncalibrate(pC) -
pc;
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) {
278 errors.segment<2>(idx) =
286 Vector errors = errorFunction(
E1,
E2, Ka, Kb, Kc);
TransferFactor(EdgeKey edge1, EdgeKey edge2, const std::vector< Triplet > &triplets, const SharedNoiseModel &model=nullptr)
Constructor that accepts a vector of point triplets.
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)
std::tuple< Point2, Point2, Point2 > Triplet
static size_t ViewB(const EdgeKey &edge1, const EdgeKey &edge2)
Returns the view B index based on the EdgeKeys.
std::tuple< Point2, Point2, Point2 > Triplet
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)
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.
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
double f2(const Vector2 &x)
Point2 EpipolarTransfer(const Matrix3 &Fca, const Point2 &pa, const Matrix3 &Fcb, const Point2 &pb)
Transfer projections from cameras a and b to camera c.
Transfers points between views using essential matrices, optimizes for calibrations of the views,...
std::shared_ptr< Factor > shared_ptr
A shared_ptr to this class.
TransferEdges(EdgeKey edge1, EdgeKey edge2)
Transfers points between views using essential matrices with a shared calibration.
EdgeKey edge2_
The two EdgeKeys.
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.
Some functions to compute numerical derivatives.
static size_t ViewC(const EdgeKey &edge1, const EdgeKey &edge2)
Returns the view C index based on the EdgeKeys.
std::vector< Triplet > triplets_
Point triplets.
std::uint32_t i() const
Retrieve high 32 bits.
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.
size_t dim() const override
Return the dimension of the factor.
static size_t ViewA(const EdgeKey &edge1, const EdgeKey &edge2)
Returns the view A index based on the EdgeKeys.
std::vector< Triplet > triplets_
Point triplets.
Vector evaluateError(const EM &E1, const EM &E2, OptionalMatrixType H1=nullptr, OptionalMatrixType H2=nullptr) const override
Evaluate error function.
noiseModel::Base::shared_ptr SharedNoiseModel
int RealScalar int RealScalar int RealScalar * pc
noiseModel::Diagonal::shared_ptr model
Non-linear factor base classes.
EssentialTransferFactorK(EdgeKey edge1, EdgeKey edge2, const std::vector< Triplet > &triplets, const SharedNoiseModel &model=nullptr)
Constructor that accepts a vector of point triplets.
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.
std::tuple< Point2, Point2, Point2 > Triplet
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)
std::uint32_t j() const
Retrieve low 32 bits.
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)
Matrix * OptionalMatrixType
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)
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)
std::shared_ptr< K > calibration_
Shared pointer to calibration object.
uint32_t c_
The transfer target.
std::pair< Matrix3, Matrix3 > getMatrices(const F &F1, const F &F2) const
Create Matrix3 objects based on EdgeKey configurations.
Vector evaluateError(const F &F1, const F &F2, OptionalMatrixType H1=nullptr, OptionalMatrixType H2=nullptr) const override
Vector of errors returns 2*N vector.
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:09:14