#ifndef _POSE_GRAPH_FUNCTION_H_
#define _POSE_GRAPH_FUNCTION_H_

#include <ceres/ceres.h>
#include <Eigen/Dense>
#include <Eigen/Cholesky>

namespace hlzn_slam {
namespace pose_graph {

class TranslationLocalParameterization : public ceres::LocalParameterization
{
    virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;
    virtual bool ComputeJacobian(const double *x, double *jacobian) const;
    virtual int GlobalSize() const { return 3; };
    virtual int LocalSize() const { return 3; };
};

class QuaternionLocalParameterization : public ceres::LocalParameterization
{
    virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;
    virtual bool ComputeJacobian(const double *x, double *jacobian) const;
    virtual int GlobalSize() const { return 4; };
    virtual int LocalSize() const { return 3; };
};

class PoseGraphFunction : public ceres::SizedCostFunction<3, 3, 4, 3, 4, 3, 4>
{
    public:
        PoseGraphFunction();
        virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;
};

}

}



#endif