Factor for Composition of Poses

[steplee]

Hello, I'd like to break the pose of a camera into a "platform" pose which is unique per time, and a "sensor" pose that is shared between all elements over time.

So I believe I want a Factor that represents the composition of the two and for my camera observations I'll transform them with the combined platform . sensor pose.

There's plenty of examples of composing Pose3 objects, but I cannot find an applicable Factor for composing group elements while tracking in the optimizable graph. BetweenFactor is only for absolute differences (whereas here we need a relative measurement, and a ternary factor?) and ReferenceFrameFactor appears to apply only to points and not poses.

Am I missing something or should I create the Factor myself, and if so, please confirm something like this makes sense and is possible?

[ProfFan]

Just make an ExpressionFactor with compose(A,B), I am pretty sure I also have one myself

[steplee]

Thanks for the quick response. I will try this and my class posted below after I get some simulated data up, and make sure one or both work!

[steplee]

Here is my ComposeFactor class. Not really tested and I've barely scratched surface of the project, but after I get to that some simulated data going I'll follow up.

#pragma once

#include <ostream>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>

namespace gtsamSteplee {
    using namespace gtsam;

    //
    // C = A . B
    //
    // error:
    //              [C . (A . B)^-1] (-) measured
    //
    // `measured` will often be zero.
    //
    // WARNING: Should we use (A.B)^-1 on the left of C instead?
    //

    template <class VALUE>
        class ComposeFactor: public NoiseModelFactorN<VALUE, VALUE, VALUE> {

            GTSAM_CONCEPT_ASSERT(IsTestable<VALUE>);
            GTSAM_CONCEPT_ASSERT(IsLieGroup<VALUE>);

            private:
            using This = ComposeFactor<VALUE>;
            using Base = NoiseModelFactorN<VALUE,VALUE,VALUE>;
            using T = VALUE;

            VALUE measured_;

            public:
            using shared_ptr = std::shared_ptr<This>;

            // This provides some overloads, we must still implement the real impl below.
            using Base::evaluateError;

            ComposeFactor() {}
            ComposeFactor(Key kc, Key ka, Key kb, const VALUE& measured, const SharedNoiseModel& model = nullptr)
                : Base(model, kc, ka, kb), measured_(measured)
            {}

            ~ComposeFactor() override {}

            NonlinearFactor::shared_ptr clone() const override {
                return std::static_pointer_cast<NonlinearFactor>(NonlinearFactor::shared_ptr(new This(*this)));
            }

            void print(
                    const std::string& s = "",
                    const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
                std::cout << s << "ComposeFactor("
                    << keyFormatter(this->key1()) << " = "
                    << keyFormatter(this->key2()) << " . "
                    << keyFormatter(this->key3()) << ")\n";
                traits<T>::Print(measured_, "  measured: ");
                this->noiseModel_->print("  noise model: ");
            }

            bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
                const This *e =  dynamic_cast<const This*> (&expected);
                return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
            }

            Vector evaluateError(const T& C, const T& A, const T& B,
                    OptionalMatrixType Hc, OptionalMatrixType Ha, OptionalMatrixType Hb) const override {

                // WARNING: not using analog of `GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR`

                // NOTE: Being very explicit/inefficient here for first pass.

                if (Hc) Hc->resize(6,6);
                if (Ha) Ha->resize(6,6);
                if (Hb) Hb->resize(6,6);

                // TODO: only compute intermediate jacobians if desired output set demand it?
                Matrix H_diff_wrt_C(6,6);
                Matrix H_diff_wrt_ABinv(6,6);

                Matrix H_ABinv_wrt_AB(6,6);
                Matrix H_AB_wrt_A(6,6);
                Matrix H_AB_wrt_B(6,6);

                T AB = traits<T>::Compose(A, B, H_AB_wrt_A, H_AB_wrt_B);
                T ABinv = traits<T>::Inverse(AB, H_ABinv_wrt_AB);

                T C_minus_AB = traits<T>::Between(C, ABinv, H_diff_wrt_C, H_diff_wrt_ABinv);

                if (Hc) (*Hc) = H_diff_wrt_C;
                if (Ha) (*Ha) = H_diff_wrt_ABinv * H_ABinv_wrt_AB * H_AB_wrt_A;
                if (Hb) (*Hb) = H_diff_wrt_ABinv * H_ABinv_wrt_AB * H_AB_wrt_B;


                return traits<T>::Local(measured_, C_minus_AB);
            }

            const VALUE& measured() const {
                return measured_;
            }


        };

    using ComposePoseFactor = ComposeFactor<Pose3>;
}

namespace gtsam {
    template<class VALUE>
        struct traits<gtsamSteplee::ComposeFactor<VALUE> > : public Testable<gtsamSteplee::ComposeFactor<VALUE> > {};
}