12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

TransferEdges base class

release/4.3a0
Frank Dellaert 2024-10-25 10:17:33 -07:00
parent 0836852dcb
commit 879bd29963
1 changed files with 56 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
gtsam/sfm/TransferFactor.h
View File

@ -22,6 +22,34 @@
namespace gtsam {
/**
* Base class that holds the EdgeKeys and provides the getMatrices method.
*/
template <typename F>
struct TransferEdges {
EdgeKey edge1, edge2; ///< The two EdgeKeys.
TransferEdges(EdgeKey edge1, EdgeKey edge2) : edge1(edge1), edge2(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 {
throw std::runtime_error(
"TransferEdges: invalid EdgeKey configuration between edge1 (" +
std::string(edge1) + ") and edge2 (" + std::string(edge2) + ").");
}
}
};
/**
* Binary factor in the context of Structure from Motion (SfM).
* It is used to transfer transfer corresponding points from two views to a
@ -30,76 +58,57 @@ namespace gtsam {
* the target view. Jacobians are done using numerical differentiation.
*/
template <typename F>
class TransferFactor : public NoiseModelFactorN<F, F> {
EdgeKey key1_, key2_; ///< the two EdgeKeys
std::vector<std::tuple<Point2, Point2, Point2>>
triplets_; ///< Point triplets
class TransferFactor : public NoiseModelFactorN<F, F>, public TransferEdges<F> {
using Base = NoiseModelFactorN<F, F>;
using Triplet = std::tuple<Point2, Point2, Point2>;
std::vector<Triplet> triplets_; ///< Point triplets.
public:
/**
* @brief Constructor for a single triplet of points
* @brief Constructor for a single triplet of points.
*
* @note: batching all points for the same transfer will be much faster.
* @note Batching all points for the same transfer will be much faster.
*
* @param key1 First EdgeKey specifying F1: (a, c) or (c, a).
* @param key2 Second EdgeKey specifying F2: (b, c) or (c, b).
* @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 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),
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.
*
* @param key1 First EdgeKey specifying F1: (a, c) or (c, a).
* @param key2 Second EdgeKey specifying F2: (b, c) or (c, b).
* @param edge1 First EdgeKey specifying F1: (a, c) or (c, a).
* @param edge2 Second EdgeKey specifying F2: (b, c) or (c, b).
* @param triplets A vector of triplets containing (pa, pb, pc).
* @param model An optional SharedNoiseModel that defines the noise model
* for this factor. Defaults to nullptr.
*/
TransferFactor(
EdgeKey key1, EdgeKey key2,
const std::vector<std::tuple<Point2, Point2, Point2>>& triplets,
TransferFactor(EdgeKey edge1, EdgeKey edge2,
const std::vector<Triplet>& triplets,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactorN<F, F>(model, key1, key2),
key1_(key1),
key2_(key2),
: Base(model, edge1, edge2),
TransferEdges<F>(edge1, edge2),
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 of errors returns 2*N vector
/// Vector of errors returns 2*N vector.
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,
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] = getMatrices(F1, F2);
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);
@ -109,9 +118,10 @@ class TransferFactor : public NoiseModelFactorN<F, F> {
}
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);
if (H1) *H1 = numericalDerivative21<Vector, F, F>(errorFunction, F1, F2);
if (H2) *H2 = numericalDerivative22<Vector, F, F>(errorFunction, F1, F2);
return errorFunction(F1, F2);
}
};