PlanarProjectionFactor.h

Go to the documentation of this file.

 
#pragma once
 
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
 
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/numericalDerivative.h>
 
 
namespace gtsam {
 
    class PlanarProjectionFactorBase {
    protected:
        PlanarProjectionFactorBase() {}
 
        PlanarProjectionFactorBase(const Point2& measured) : measured_(measured) {}
 
        Point2 predict(
            const Point3& landmark,
            const Pose2& wTb,
            const Pose3& bTc,
            const Cal3DS2& calib,
            OptionalJacobian<2, 3> Hlandmark = {}, // (x, y, z)
            OptionalJacobian<2, 3> HwTb = {}, // (x, y, theta)
            OptionalJacobian<2, 6> HbTc = {}, // (rx, ry, rz, x, y, theta)
            OptionalJacobian<2, 9> Hcalib = {}
        ) const {
#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
            try {
#endif
                Matrix63 Hp; // 6x3
                Matrix66 H0; // 6x6
                Pose3 wTc = Pose3::FromPose2(wTb, HwTb ? &Hp : nullptr).compose(bTc, HwTb ? &H0 : nullptr);
                PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(wTc, calib);
                if (HwTb || HbTc) {
                    // Dpose is for pose3 (R,t)
                    Matrix26 Dpose;
                    Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib);
                    if (HbTc)
                        *HbTc = Dpose;
                    if (HwTb)
                        *HwTb = Dpose * H0 * Hp;
                    return result;
                } else {
                    return camera.project(landmark, {}, {}, {});
                }
#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
            } catch (CheiralityException& e) {
                std::cout << "****** CHIRALITY EXCEPTION ******\n";
                if (Hlandmark) Hlandmark->setZero();
                if (HwTb) HwTb->setZero();
                if (HbTc) HbTc->setZero();
                if (Hcalib) Hcalib->setZero();
                // return a large error
                return Matrix::Constant(2, 1, 2.0 * calib.fx());
            }
#endif
        }
 
        Point2 measured_; // pixel measurement
    };
 
 
    class PlanarProjectionFactor1
        : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2> {
    public:
        typedef NoiseModelFactorN<Pose2> Base;
        using Base::evaluateError;
        PlanarProjectionFactor1() {}
 
        ~PlanarProjectionFactor1() override {}
 
        NonlinearFactor::shared_ptr clone() const override {
            return std::static_pointer_cast<NonlinearFactor>(
                NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this)));
        }
 
 
        PlanarProjectionFactor1(
            Key poseKey,
            const Point3& landmark,
            const Point2& measured,
            const Pose3& bTc,
            const Cal3DS2& calib,
            const SharedNoiseModel& model = {})
            : PlanarProjectionFactorBase(measured),
            NoiseModelFactorN(model, poseKey),
            landmark_(landmark),
            bTc_(bTc),
            calib_(calib) {}
 
        Vector evaluateError(const Pose2& wTb, OptionalMatrixType HwTb) const override {
            return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_;
        }
 
    private:
        Point3 landmark_; // landmark
        Pose3 bTc_; // "body to camera": camera offset to robot pose
        Cal3DS2 calib_; // camera calibration
    };
 
    template<>
    struct traits<PlanarProjectionFactor1> :
        public Testable<PlanarProjectionFactor1> {};
 
    class PlanarProjectionFactor2
        : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Point3> {
    public:
        typedef NoiseModelFactorN<Pose2, Point3> Base;
        using Base::evaluateError;
 
        PlanarProjectionFactor2() {}
 
        ~PlanarProjectionFactor2() override {}
 
        NonlinearFactor::shared_ptr clone() const override {
            return std::static_pointer_cast<NonlinearFactor>(
                NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this)));
        }
 
        PlanarProjectionFactor2(
            Key poseKey,
            Key landmarkKey,
            const Point2& measured,
            const Pose3& bTc,
            const Cal3DS2& calib,
            const SharedNoiseModel& model = {})
            : PlanarProjectionFactorBase(measured),
            NoiseModelFactorN(model, landmarkKey, poseKey),
            bTc_(bTc),
            calib_(calib) {}
 
        Vector evaluateError(
            const Pose2& wTb,
            const Point3& landmark,
            OptionalMatrixType HwTb,
            OptionalMatrixType Hlandmark) const override {
            return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_;
        }
 
    private:
        Pose3 bTc_; // "body to camera": camera offset to robot pose
        Cal3DS2 calib_; // camera calibration
    };
 
    template<>
    struct traits<PlanarProjectionFactor2> :
        public Testable<PlanarProjectionFactor2> {};
 
    class PlanarProjectionFactor3 : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Pose3, Cal3DS2> {
    public:
        typedef NoiseModelFactorN<Pose2, Pose3, Cal3DS2> Base;
        using Base::evaluateError;
 
        PlanarProjectionFactor3() {}
 
        ~PlanarProjectionFactor3() override {}
 
        NonlinearFactor::shared_ptr clone() const override {
            return std::static_pointer_cast<NonlinearFactor>(
                NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this)));
        }
 
        PlanarProjectionFactor3(
            Key poseKey,
            Key offsetKey,
            Key calibKey,
            const Point3& landmark,
            const Point2& measured,
            const SharedNoiseModel& model = {})
            : PlanarProjectionFactorBase(measured),
            NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
            landmark_(landmark) {}
 
        Vector evaluateError(
            const Pose2& wTb,
            const Pose3& bTc,
            const Cal3DS2& calib,
            OptionalMatrixType HwTb,
            OptionalMatrixType HbTc,
            OptionalMatrixType Hcalib) const override {
            return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_;
        }
 
    private:
        Point3 landmark_; // landmark
    };
 
    template<>
    struct traits<PlanarProjectionFactor3> :
        public Testable<PlanarProjectionFactor3> {};
 
} // namespace gtsam