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

#ifndef GTSAM_TEST_EDGE3FACTOR_H
#define GTSAM_TEST_EDGE3FACTOR_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>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/ISAM2.h>
using namespace gtsam;
class Edge3Factor :public  gtsam::NoiseModelFactor3<double,double,double>{
public:
    Edge3Factor(Key k1, Key k2,Key k3,double a, double b, const SharedNoiseModel& model):
      NoiseModelFactor3<double,double,double>(model, k1,k2,k3), pa_(a), pb_(b) {}

    virtual Vector
    evaluateError(const double& x, const double& y, const double& z,
                  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 Edge3Factor(*this)));
    }

protected:
    double pa_;
    double pb_;

};

class Edge3Factor2 :public  gtsam::NoiseModelFactor3<double,double,double>{
public:
    Edge3Factor2(Key k1, Key k2,Key k3,double a, double b, const SharedNoiseModel& model):
        NoiseModelFactor3<double,double,double>(model, k1,k2,k3), pa_(a), pb_(b) {}

    virtual Vector
    evaluateError(const double& x, const double& y, const double& z,
                  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 Edge3Factor2(*this)));
    }

protected:
    double pa_;
    double pb_;

};

#endif //GTSAM_TEST_EDGE3FACTOR_H