//
// Created by zx on 23-6-20.
//

#ifndef GTSAM_TEST_POSESPEEDFACTOR_H
#define GTSAM_TEST_POSESPEEDFACTOR_H
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
using namespace gtsam;
class PoseSpeedFactor :public gtsam::NoiseModelFactor3<Pose2,Vector2,Pose2>{
public:
    PoseSpeedFactor(Key p1, Key v1,Key p2,Vector3 dt, const SharedNoiseModel& model):
      NoiseModelFactor3<Pose2,Vector2,Pose2>(model, p1,v1,p2), dt_(dt) {}

    virtual Vector evaluateError(const Pose2& p1, const Vector2 & v1, const Pose2& p2,
                  boost::optional<Matrix&> H1 = boost::none,
                  boost::optional<Matrix&> H2 = boost::none,
                  boost::optional<Matrix&> H3 = boost::none) const;

    virtual gtsam::NonlinearFactor::shared_ptr clone() const
    {
      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
          gtsam::NonlinearFactor::shared_ptr(new PoseSpeedFactor(*this)));
    }
protected:
    Vector3 dt_;

};

#endif //GTSAM_TEST_POSESPEEDFACTOR_H