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

102
gtsam/sfm/TransferFactor.h
View File

@ -22,6 +22,34 @@
namespace gtsam { 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). * Binary factor in the context of Structure from Motion (SfM).
* It is used to transfer transfer corresponding points from two views to a * 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. * the target view. Jacobians are done using numerical differentiation.
*/ */
template <typename F> template <typename F>
class TransferFactor : public NoiseModelFactorN<F, F> { class TransferFactor : public NoiseModelFactorN<F, F>, public TransferEdges<F> {
EdgeKey key1_, key2_; ///< the two EdgeKeys using Base = NoiseModelFactorN<F, F>;
std::vector<std::tuple<Point2, Point2, Point2>> using Triplet = std::tuple<Point2, Point2, Point2>;
triplets_; ///< Point triplets std::vector<Triplet> triplets_; ///< Point triplets.
public: 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 edge1 First EdgeKey specifying F1: (a, c) or (c, a).
* @param key2 Second EdgeKey specifying F2: (b, c) or (c, b). * @param edge2 Second EdgeKey specifying F2: (b, c) or (c, b).
* @param pa The point in the first view (a). * @param pa The point in the first view (a).
* @param pb The point in the second view (b). * @param pb The point in the second view (b).
* @param pc The point in the third (and transfer target) view (c). * @param pc The point in the third (and transfer target) view (c).
* @param model An optional SharedNoiseModel that defines the noise model * @param model An optional SharedNoiseModel that defines the noise model
* for this factor. Defaults to nullptr. * for this factor. Defaults to nullptr.
*/ */
TransferFactor(EdgeKey key1, EdgeKey key2, const Point2& pa, const Point2& pb, TransferFactor(EdgeKey edge1, EdgeKey edge2, const Point2& pa,
const Point2& pc, const SharedNoiseModel& model = nullptr) const Point2& pb, const Point2& pc,
: NoiseModelFactorN<F, F>(model, key1, key2), const SharedNoiseModel& model = nullptr)
key1_(key1), : Base(model, edge1, edge2),
key2_(key2), TransferEdges<F>(edge1, edge2),
triplets_({std::make_tuple(pa, pb, pc)}) {} triplets_({std::make_tuple(pa, pb, pc)}) {}
/** /**
* @brief Constructor that accepts a vector of point triplets. * @brief Constructor that accepts a vector of point triplets.
* *
* @param key1 First EdgeKey specifying F1: (a, c) or (c, a). * @param edge1 First EdgeKey specifying F1: (a, c) or (c, a).
* @param key2 Second EdgeKey specifying F2: (b, c) or (c, b). * @param edge2 Second EdgeKey specifying F2: (b, c) or (c, b).
* @param triplets A vector of triplets containing (pa, pb, pc). * @param triplets A vector of triplets containing (pa, pb, pc).
* @param model An optional SharedNoiseModel that defines the noise model * @param model An optional SharedNoiseModel that defines the noise model
* for this factor. Defaults to nullptr. * for this factor. Defaults to nullptr.
*/ */
TransferFactor( TransferFactor(EdgeKey edge1, EdgeKey edge2,
EdgeKey key1, EdgeKey key2, const std::vector<Triplet>& triplets,
const std::vector<std::tuple<Point2, Point2, Point2>>& triplets, const SharedNoiseModel& model = nullptr)
const SharedNoiseModel& model = nullptr) : Base(model, edge1, edge2),
: NoiseModelFactorN<F, F>(model, key1, key2), TransferEdges<F>(edge1, edge2),
key1_(key1),
key2_(key2),
triplets_(triplets) {} triplets_(triplets) {}
// Create Matrix3 objects based on EdgeKey configurations /// Vector of errors returns 2*N vector.
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 evaluateError(const F& F1, const F& F2, Vector evaluateError(const F& F1, const F& F2,
OptionalMatrixType H1 = nullptr, OptionalMatrixType H1 = nullptr,
OptionalMatrixType H2 = nullptr) const override { 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) { const F& F2) {
Vector errors(2 * triplets_.size()); Vector errors(2 * triplets_.size());
size_t idx = 0; size_t idx = 0;
auto [Fca, Fcb] = getMatrices(F1, F2); auto [Fca, Fcb] = this->getMatrices(F1, F2);
for (const auto& tuple : triplets_) { for (const auto& tuple : triplets_) {
const auto& [pa, pb, pc] = tuple; const auto& [pa, pb, pc] = tuple;
Point2 transferredPoint = EpipolarTransfer(Fca, pa, Fcb, pb); Point2 transferredPoint = EpipolarTransfer(Fca, pa, Fcb, pb);
@ -109,9 +118,10 @@ class TransferFactor : public NoiseModelFactorN<F, F> {
} }
return errors; return errors;
}; };
if (H1) *H1 = numericalDerivative21<Vector, F, F>(transfer, F1, F2); if (H1) *H1 = numericalDerivative21<Vector, F, F>(errorFunction, F1, F2);
if (H2) *H2 = numericalDerivative22<Vector, F, F>(transfer, F1, F2); if (H2) *H2 = numericalDerivative22<Vector, F, F>(errorFunction, F1, F2);
return transfer(F1, F2); return errorFunction(F1, F2);
} }
}; };