#ifndef _POSE_COST_FUNCTION_H_
#define _POSE_COST_FUNCTION_H_

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

namespace hlzn_slam {
namespace pose_graph {

class PoseLocalParameterization : 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 7; };
    virtual int LocalSize() const { return 6; };
};

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

}

}



#endif