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

#ifndef GTSAM_TEST_SPEEDFACTOR_H
#define GTSAM_TEST_SPEEDFACTOR_H
#include <ostream>

#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose2.h>
using namespace gtsam;
class SpeedFactor: public NoiseModelFactor2<gtsam::Pose2, gtsam::Pose2>{
public:
    SpeedFactor(Key i,Key j,double v,double w,const SharedNoiseModel& model):
      v_(v),w_(w),NoiseModelFactor2<Pose2,Pose2>(model,i,j){
    }
    virtual ~SpeedFactor(){}

    Vector evaluateError(const Pose2& p1, const Pose2& p2, boost::optional<Matrix&> H1 =
    boost::none, boost::optional<Matrix&> H2 = boost::none) const {

    }


protected:
    double v_;
    double w_;
};

#endif //GTSAM_TEST_SPEEDFACTOR_H